// Copyright 2021-2023, Collabora, Ltd. // SPDX-License-Identifier: BSL-1.0 /*! * @file * @brief Camera based hand tracking driver code. * @author Moshi Turner * @author Jakob Bornecrantz * @ingroup drv_ht */ #include "ht_interface.h" #include "tracking/t_tracking.h" #include "util/u_var.h" #include "xrt/xrt_defines.h" #include "xrt/xrt_frame.h" #include "xrt/xrt_frameserver.h" #include "xrt/xrt_prober.h" #include "util/u_device.h" #include "util/u_logging.h" #include "util/u_trace_marker.h" #include "util/u_config_json.h" #include "util/u_debug.h" #include "util/u_sink.h" #include "util/u_file.h" #include "tracking/t_hand_tracking.h" // Save me, Obi-Wan! #include "../../tracking/hand/mercury/hg_interface.h" #ifdef XRT_BUILD_DRIVER_DEPTHAI #include "../depthai/depthai_interface.h" #endif #include DEBUG_GET_ONCE_LOG_OPTION(ht_log, "HT_LOG", U_LOGGING_WARN) #define HT_TRACE(htd, ...) U_LOG_XDEV_IFL_T(&htd->base, htd->log_level, __VA_ARGS__) #define HT_DEBUG(htd, ...) U_LOG_XDEV_IFL_D(&htd->base, htd->log_level, __VA_ARGS__) #define HT_INFO(htd, ...) U_LOG_XDEV_IFL_I(&htd->base, htd->log_level, __VA_ARGS__) #define HT_WARN(htd, ...) U_LOG_XDEV_IFL_W(&htd->base, htd->log_level, __VA_ARGS__) #define HT_ERROR(htd, ...) U_LOG_XDEV_IFL_E(&htd->base, htd->log_level, __VA_ARGS__) struct ht_device { struct xrt_device base; //! Whether to use our `xfctx` or an externally managed one. //! @note This variable exists because we still need to settle on the ht usage interface. bool own_xfctx; struct xrt_frame_context xfctx; struct t_hand_tracking_sync *sync; struct t_hand_tracking_async *async; enum u_logging_level log_level; }; static inline struct ht_device * ht_device(struct xrt_device *xdev) { return (struct ht_device *)xdev; } #if 0 static void getStartupConfig(struct ht_device *htd, const cJSON *startup_config) { const cJSON *uvc_wire_format = u_json_get(startup_config, "uvc_wire_format"); if (cJSON_IsString(uvc_wire_format)) { bool is_yuv = (strcmp(cJSON_GetStringValue(uvc_wire_format), "yuv") == 0); bool is_mjpeg = (strcmp(cJSON_GetStringValue(uvc_wire_format), "mjpeg") == 0); if (!is_yuv && !is_mjpeg) { HT_WARN(htd, "Unknown wire format type %s - should be \"yuv\" or \"mjpeg\"", cJSON_GetStringValue(uvc_wire_format)); } if (is_yuv) { HT_DEBUG(htd, "Using YUYV422!"); htd->desired_format = XRT_FORMAT_YUYV422; } else { HT_DEBUG(htd, "Using MJPEG!"); htd->desired_format = XRT_FORMAT_MJPEG; } } } static void getUserConfig(struct ht_device *htd) { // The game here is to avoid bugs + be cautious, not to be fast. If you see something that seems "slow" - don't // fix it. Any of the tracking code is way stickier than this could ever be. struct u_config_json config_json = {0}; u_config_json_open_or_create_main_file(&config_json); if (!config_json.file_loaded) { return; } cJSON *ht_config_json = cJSON_GetObjectItemCaseSensitive(config_json.root, "config_ht"); if (ht_config_json == NULL) { return; } // Don't get it twisted: initializing these to NULL is not cargo-culting. // Uninitialized values on the stack aren't guaranteed to be 0, so these could end up pointing to what we // *think* is a valid address but what is *not* one. char *startup_config_string = NULL; { const cJSON *startup_config_string_json = u_json_get(ht_config_json, "startup_config_index"); if (cJSON_IsString(startup_config_string_json)) { startup_config_string = cJSON_GetStringValue(startup_config_string_json); } } if (startup_config_string != NULL) { const cJSON *startup_config_obj = u_json_get(u_json_get(ht_config_json, "startup_configs"), startup_config_string); getStartupConfig(htd, startup_config_obj); } cJSON_Delete(config_json.root); return; } static void userConfigSetDefaults(struct ht_device *htd) { htd->desired_format = XRT_FORMAT_YUYV422; } #endif /*! * xrt_device function implementations */ static xrt_result_t ht_device_get_hand_tracking(struct xrt_device *xdev, enum xrt_input_name name, int64_t at_timestamp_ns, struct xrt_hand_joint_set *out_value, int64_t *out_timestamp_ns) { struct ht_device *htd = ht_device(xdev); if (name != XRT_INPUT_HT_UNOBSTRUCTED_LEFT && name != XRT_INPUT_HT_UNOBSTRUCTED_RIGHT) { U_LOG_XDEV_UNSUPPORTED_INPUT(&htd->base, htd->log_level, name); return XRT_ERROR_INPUT_UNSUPPORTED; } htd->async->get_hand(htd->async, name, at_timestamp_ns, out_value, out_timestamp_ns); return XRT_SUCCESS; } static void ht_device_destroy(struct xrt_device *xdev) { struct ht_device *htd = ht_device(xdev); HT_DEBUG(htd, "called!"); if (htd->own_xfctx) { xrt_frame_context_destroy_nodes(&htd->xfctx); } // Remove the variable tracking. u_var_remove_root(htd); u_device_free(&htd->base); } static struct ht_device * ht_device_create_common(struct t_stereo_camera_calibration *calib, bool own_xfctx, struct xrt_frame_context *xfctx, struct t_hand_tracking_sync *sync) { XRT_TRACE_MARKER(); enum u_device_alloc_flags flags = U_DEVICE_ALLOC_NO_FLAGS | U_DEVICE_ALLOC_TRACKING_NONE; //! @todo 2 hands hardcoded int num_hands = 2; // Allocate device struct ht_device *htd = U_DEVICE_ALLOCATE(struct ht_device, flags, num_hands, 0); // Setup logging first htd->log_level = debug_get_log_option_ht_log(); htd->own_xfctx = own_xfctx; if (own_xfctx) { // Transfer ownership of xfctx to htd htd->xfctx.nodes = xfctx->nodes; } htd->base.tracking_origin->type = XRT_TRACKING_TYPE_RGB; htd->base.update_inputs = u_device_noop_update_inputs; htd->base.get_hand_tracking = ht_device_get_hand_tracking; htd->base.destroy = ht_device_destroy; snprintf(htd->base.str, XRT_DEVICE_NAME_LEN, "Camera based Hand Tracker"); snprintf(htd->base.serial, XRT_DEVICE_NAME_LEN, "Camera based Hand Tracker"); htd->base.inputs[0].name = XRT_INPUT_HT_UNOBSTRUCTED_LEFT; htd->base.inputs[1].name = XRT_INPUT_HT_UNOBSTRUCTED_RIGHT; // Yes, you need all of these. Yes, I tried disabling them all one at a time. You need all of these. htd->base.name = XRT_DEVICE_HAND_TRACKER; htd->base.device_type = XRT_DEVICE_TYPE_HAND_TRACKER; htd->base.supported.orientation_tracking = true; htd->base.supported.position_tracking = true; htd->base.supported.hand_tracking = true; htd->sync = sync; htd->async = t_hand_tracking_async_default_create(xfctx, sync); return htd; } int ht_device_create(struct xrt_frame_context *xfctx, struct t_stereo_camera_calibration *calib, struct t_hand_tracking_create_info create_info, struct xrt_slam_sinks **out_sinks, struct xrt_device **out_device) { XRT_TRACE_MARKER(); assert(calib != NULL); struct t_hand_tracking_sync *sync = NULL; char path[1024] = {0}; int ret = u_file_get_hand_tracking_models_dir(path, ARRAY_SIZE(path)); if (ret < 0) { U_LOG_E( "Could not find any directory with hand-tracking models!\n\t" "Run ./scripts/get-ht-models.sh or install monado-data package"); return -1; } sync = t_hand_tracking_sync_mercury_create(calib, create_info, path); struct ht_device *htd = ht_device_create_common(calib, false, xfctx, sync); HT_DEBUG(htd, "Hand Tracker initialized!"); *out_sinks = &htd->async->sinks; *out_device = &htd->base; return 0; }