matrixfurry.com
The open source OpenXR runtime
1// Copyright 2021-2023, Collabora, Ltd.
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief Camera based hand tracking driver code.
6 * @author Moshi Turner <moshiturner@protonmail.com>
7 * @author Jakob Bornecrantz <jakob@collabora.com>
8 * @ingroup drv_ht
9 */
10
11#include "ht_interface.h"
12
13
14#include "tracking/t_tracking.h"
15#include "util/u_var.h"
16#include "xrt/xrt_defines.h"
17#include "xrt/xrt_frame.h"
18#include "xrt/xrt_frameserver.h"
19#include "xrt/xrt_prober.h"
20
21#include "util/u_device.h"
22#include "util/u_logging.h"
23#include "util/u_trace_marker.h"
24#include "util/u_config_json.h"
25#include "util/u_debug.h"
26#include "util/u_sink.h"
27#include "util/u_file.h"
28
29#include "tracking/t_hand_tracking.h"
30
31// Save me, Obi-Wan!
32
33#include "../../tracking/hand/mercury/hg_interface.h"
34
35#ifdef XRT_BUILD_DRIVER_DEPTHAI
36#include "../depthai/depthai_interface.h"
37#endif
38
39
40#include <cjson/cJSON.h>
41
42DEBUG_GET_ONCE_LOG_OPTION(ht_log, "HT_LOG", U_LOGGING_WARN)
43
44
45#define HT_TRACE(htd, ...) U_LOG_XDEV_IFL_T(&htd->base, htd->log_level, __VA_ARGS__)
46#define HT_DEBUG(htd, ...) U_LOG_XDEV_IFL_D(&htd->base, htd->log_level, __VA_ARGS__)
47#define HT_INFO(htd, ...) U_LOG_XDEV_IFL_I(&htd->base, htd->log_level, __VA_ARGS__)
48#define HT_WARN(htd, ...) U_LOG_XDEV_IFL_W(&htd->base, htd->log_level, __VA_ARGS__)
49#define HT_ERROR(htd, ...) U_LOG_XDEV_IFL_E(&htd->base, htd->log_level, __VA_ARGS__)
50
51
52
53struct ht_device
54{
55 struct xrt_device base;
56
57 //! Whether to use our `xfctx` or an externally managed one.
58 //! @note This variable exists because we still need to settle on the ht usage interface.
59 bool own_xfctx;
60 struct xrt_frame_context xfctx;
61
62 struct t_hand_tracking_sync *sync;
63 struct t_hand_tracking_async *async;
64
65 enum u_logging_level log_level;
66};
67
68static inline struct ht_device *
69ht_device(struct xrt_device *xdev)
70{
71 return (struct ht_device *)xdev;
72}
73
74#if 0
75static void
76getStartupConfig(struct ht_device *htd, const cJSON *startup_config)
77{
78 const cJSON *uvc_wire_format = u_json_get(startup_config, "uvc_wire_format");
79
80 if (cJSON_IsString(uvc_wire_format)) {
81 bool is_yuv = (strcmp(cJSON_GetStringValue(uvc_wire_format), "yuv") == 0);
82 bool is_mjpeg = (strcmp(cJSON_GetStringValue(uvc_wire_format), "mjpeg") == 0);
83 if (!is_yuv && !is_mjpeg) {
84 HT_WARN(htd, "Unknown wire format type %s - should be \"yuv\" or \"mjpeg\"",
85 cJSON_GetStringValue(uvc_wire_format));
86 }
87 if (is_yuv) {
88 HT_DEBUG(htd, "Using YUYV422!");
89 htd->desired_format = XRT_FORMAT_YUYV422;
90 } else {
91 HT_DEBUG(htd, "Using MJPEG!");
92 htd->desired_format = XRT_FORMAT_MJPEG;
93 }
94 }
95}
96
97static void
98getUserConfig(struct ht_device *htd)
99{
100 // The game here is to avoid bugs + be cautious, not to be fast. If you see something that seems "slow" - don't
101 // fix it. Any of the tracking code is way stickier than this could ever be.
102
103 struct u_config_json config_json = {0};
104
105 u_config_json_open_or_create_main_file(&config_json);
106 if (!config_json.file_loaded) {
107 return;
108 }
109
110 cJSON *ht_config_json = cJSON_GetObjectItemCaseSensitive(config_json.root, "config_ht");
111 if (ht_config_json == NULL) {
112 return;
113 }
114
115 // Don't get it twisted: initializing these to NULL is not cargo-culting.
116 // Uninitialized values on the stack aren't guaranteed to be 0, so these could end up pointing to what we
117 // *think* is a valid address but what is *not* one.
118 char *startup_config_string = NULL;
119
120 {
121 const cJSON *startup_config_string_json = u_json_get(ht_config_json, "startup_config_index");
122 if (cJSON_IsString(startup_config_string_json)) {
123 startup_config_string = cJSON_GetStringValue(startup_config_string_json);
124 }
125 }
126
127 if (startup_config_string != NULL) {
128 const cJSON *startup_config_obj =
129 u_json_get(u_json_get(ht_config_json, "startup_configs"), startup_config_string);
130 getStartupConfig(htd, startup_config_obj);
131 }
132
133 cJSON_Delete(config_json.root);
134 return;
135}
136
137static void
138userConfigSetDefaults(struct ht_device *htd)
139{
140 htd->desired_format = XRT_FORMAT_YUYV422;
141}
142#endif
143
144/*!
145 * xrt_device function implementations
146 */
147
148static xrt_result_t
149ht_device_get_hand_tracking(struct xrt_device *xdev,
150 enum xrt_input_name name,
151 int64_t at_timestamp_ns,
152 struct xrt_hand_joint_set *out_value,
153 int64_t *out_timestamp_ns)
154{
155 struct ht_device *htd = ht_device(xdev);
156
157 if (name != XRT_INPUT_HT_UNOBSTRUCTED_LEFT && name != XRT_INPUT_HT_UNOBSTRUCTED_RIGHT) {
158 U_LOG_XDEV_UNSUPPORTED_INPUT(&htd->base, htd->log_level, name);
159 return XRT_ERROR_INPUT_UNSUPPORTED;
160 }
161
162 htd->async->get_hand(htd->async, name, at_timestamp_ns, out_value, out_timestamp_ns);
163 return XRT_SUCCESS;
164}
165
166static void
167ht_device_destroy(struct xrt_device *xdev)
168{
169 struct ht_device *htd = ht_device(xdev);
170 HT_DEBUG(htd, "called!");
171
172 if (htd->own_xfctx) {
173 xrt_frame_context_destroy_nodes(&htd->xfctx);
174 }
175
176 // Remove the variable tracking.
177 u_var_remove_root(htd);
178
179 u_device_free(&htd->base);
180}
181
182static struct ht_device *
183ht_device_create_common(struct t_stereo_camera_calibration *calib,
184 bool own_xfctx,
185 struct xrt_frame_context *xfctx,
186 struct t_hand_tracking_sync *sync)
187{
188 XRT_TRACE_MARKER();
189
190 enum u_device_alloc_flags flags = U_DEVICE_ALLOC_NO_FLAGS | U_DEVICE_ALLOC_TRACKING_NONE;
191
192 //! @todo 2 hands hardcoded
193 int num_hands = 2;
194
195 // Allocate device
196 struct ht_device *htd = U_DEVICE_ALLOCATE(struct ht_device, flags, num_hands, 0);
197
198 // Setup logging first
199 htd->log_level = debug_get_log_option_ht_log();
200
201 htd->own_xfctx = own_xfctx;
202 if (own_xfctx) { // Transfer ownership of xfctx to htd
203 htd->xfctx.nodes = xfctx->nodes;
204 }
205
206 htd->base.tracking_origin->type = XRT_TRACKING_TYPE_RGB;
207
208 htd->base.update_inputs = u_device_noop_update_inputs;
209 htd->base.get_hand_tracking = ht_device_get_hand_tracking;
210 htd->base.destroy = ht_device_destroy;
211
212 snprintf(htd->base.str, XRT_DEVICE_NAME_LEN, "Camera based Hand Tracker");
213 snprintf(htd->base.serial, XRT_DEVICE_NAME_LEN, "Camera based Hand Tracker");
214
215 htd->base.inputs[0].name = XRT_INPUT_HT_UNOBSTRUCTED_LEFT;
216 htd->base.inputs[1].name = XRT_INPUT_HT_UNOBSTRUCTED_RIGHT;
217
218 // Yes, you need all of these. Yes, I tried disabling them all one at a time. You need all of these.
219 htd->base.name = XRT_DEVICE_HAND_TRACKER;
220 htd->base.device_type = XRT_DEVICE_TYPE_HAND_TRACKER;
221 htd->base.supported.orientation_tracking = true;
222 htd->base.supported.position_tracking = true;
223 htd->base.supported.hand_tracking = true;
224
225 htd->sync = sync;
226
227 htd->async = t_hand_tracking_async_default_create(xfctx, sync);
228 return htd;
229}
230
231int
232ht_device_create(struct xrt_frame_context *xfctx,
233 struct t_stereo_camera_calibration *calib,
234 struct t_hand_tracking_create_info create_info,
235 struct xrt_slam_sinks **out_sinks,
236 struct xrt_device **out_device)
237{
238
239 XRT_TRACE_MARKER();
240 assert(calib != NULL);
241
242 struct t_hand_tracking_sync *sync = NULL;
243
244 char path[1024] = {0};
245
246 int ret = u_file_get_hand_tracking_models_dir(path, ARRAY_SIZE(path));
247 if (ret < 0) {
248 U_LOG_E(
249 "Could not find any directory with hand-tracking models!\n\t"
250 "Run ./scripts/get-ht-models.sh or install monado-data package");
251 return -1;
252 }
253
254 sync = t_hand_tracking_sync_mercury_create(calib, create_info, path);
255
256 struct ht_device *htd = ht_device_create_common(calib, false, xfctx, sync);
257
258 HT_DEBUG(htd, "Hand Tracker initialized!");
259
260 *out_sinks = &htd->async->sinks;
261 *out_device = &htd->base;
262
263 return 0;
264}