From 3cbb7f6b9f6df75d6cd8b4c2cd18c4a35208be25 Mon Sep 17 00:00:00 2001 From: Mateo de Mayo Date: Wed, 20 Jul 2022 13:12:10 -0300 Subject: [PATCH] d/vive: Pass status of visual tracking into the driver --- src/xrt/drivers/vive/vive_device.c | 91 +++++++++++++++---- src/xrt/drivers/vive/vive_device.h | 37 +++++++- src/xrt/drivers/vive/vive_prober.c | 27 +++--- src/xrt/drivers/vive/vive_prober.h | 3 + src/xrt/drivers/wmr/wmr_hmd.c | 2 +- .../common/target_builder_lighthouse.c | 58 +++++++++--- 6 files changed, 170 insertions(+), 48 deletions(-) diff --git a/src/xrt/drivers/vive/vive_device.c b/src/xrt/drivers/vive/vive_device.c index d46960c4b..f8bdf512d 100644 --- a/src/xrt/drivers/vive/vive_device.c +++ b/src/xrt/drivers/vive/vive_device.c @@ -30,7 +30,6 @@ #include "vive_protocol.h" - DEBUG_GET_ONCE_LOG_OPTION(vive_log, "VIVE_LOG", U_LOGGING_WARN) static bool @@ -702,6 +701,26 @@ vive_sensors_run_thread(void *ptr) return NULL; } +static void +vive_device_setup_ui(struct vive_device *d) +{ + u_var_add_root(d, "Vive Device", true); + + u_var_add_gui_header(d, &d->gui.fusion, "3DoF Tracking"); + m_imu_3dof_add_vars(&d->fusion.i3dof, d, ""); + u_var_add_gui_header(d, &d->gui.calibration, "Calibration"); + u_var_add_vec3_f32(d, &d->config.imu.acc_scale, "acc_scale"); + u_var_add_vec3_f32(d, &d->config.imu.acc_bias, "acc_bias"); + u_var_add_vec3_f32(d, &d->config.imu.gyro_scale, "gyro_scale"); + u_var_add_vec3_f32(d, &d->config.imu.gyro_bias, "gyro_bias"); + + u_var_add_gui_header(d, NULL, "SLAM Tracking"); + u_var_add_ro_text(d, d->gui.slam_status, "Tracker status"); + + u_var_add_gui_header(d, NULL, "Hand Tracking"); + u_var_add_ro_text(d, d->gui.hand_status, "Tracker status"); +} + static bool compute_distortion(struct xrt_device *xdev, int view, float u, float v, struct xrt_uv_triplet *result) { @@ -711,11 +730,61 @@ compute_distortion(struct xrt_device *xdev, int view, float u, float v, struct x return u_compute_distortion_vive(&d->config.distortion[view], u, v, result); } +static bool +vive_setup_trackers(struct vive_device *d, struct vive_tracking_status status) +{ + // Tracking status setup + bool dof3_enabled = true; // We always have at least 3dof HMD tracking + bool slam_wanted = status.slam_wanted; + bool slam_supported = status.slam_supported; + bool slam_enabled = status.slam_enabled; + bool hand_wanted = status.hand_wanted; + bool hand_supported = status.hand_supported; + bool hand_enabled = status.hand_enabled; + + d->base.orientation_tracking_supported = dof3_enabled || slam_enabled; + d->base.position_tracking_supported = slam_enabled; + d->base.hand_tracking_supported = false; // this is handled by a separate hand device + d->base.device_type = XRT_DEVICE_TYPE_HMD; + + d->tracking.slam_enabled = slam_enabled; + d->tracking.hand_enabled = hand_enabled; + + d->slam_over_3dof = slam_enabled; // We prefer SLAM over 3dof tracking if possible + + const char *slam_status = d->tracking.slam_enabled ? "Enabled" + : !slam_wanted ? "Disabled by the user (envvar set to false)" + : !slam_supported ? "Unavailable (not built)" + : NULL; + + const char *hand_status = d->tracking.hand_enabled ? "Enabled" + : !hand_wanted ? "Disabled by the user (envvar set to false)" + : !hand_supported ? "Unavailable (not built)" + : NULL; + + assert(slam_status != NULL && hand_status != NULL); + + snprintf(d->gui.slam_status, sizeof(d->gui.slam_status), "%s", slam_status); + snprintf(d->gui.hand_status, sizeof(d->gui.hand_status), "%s", hand_status); + + // Initialize 3DoF tracker + m_imu_3dof_init(&d->fusion.i3dof, M_IMU_3DOF_USE_GRAVITY_DUR_20MS); + + int ret = os_mutex_init(&d->fusion.mutex); + if (ret != 0) { + VIVE_ERROR(d, "Failed to init 3dof mutex"); + return false; + } + + return true; +} + struct vive_device * vive_device_create(struct os_hid_device *mainboard_dev, struct os_hid_device *sensors_dev, struct os_hid_device *watchman_dev, - enum VIVE_VARIANT variant) + enum VIVE_VARIANT variant, + struct vive_tracking_status tstatus) { XRT_TRACE_MARKER(); @@ -837,18 +906,6 @@ vive_device_create(struct os_hid_device *mainboard_dev, os_thread_helper_init(&d->sensors_thread); os_thread_helper_init(&d->watchman_thread); - // Init here. - m_imu_3dof_init(&d->fusion.i3dof, M_IMU_3DOF_USE_GRAVITY_DUR_20MS); - - u_var_add_root(d, "Vive Device", true); - u_var_add_gui_header(d, &d->gui.fusion, "3DoF Fusion"); - m_imu_3dof_add_vars(&d->fusion.i3dof, d, ""); - u_var_add_gui_header(d, &d->gui.calibration, "Calibration"); - u_var_add_vec3_f32(d, &d->config.imu.acc_scale, "acc_scale"); - u_var_add_vec3_f32(d, &d->config.imu.acc_bias, "acc_bias"); - u_var_add_vec3_f32(d, &d->config.imu.gyro_scale, "gyro_scale"); - u_var_add_vec3_f32(d, &d->config.imu.gyro_bias, "gyro_bias"); - int ret; if (watchman_dev != NULL) { @@ -870,9 +927,7 @@ vive_device_create(struct os_hid_device *mainboard_dev, } } - d->base.orientation_tracking_supported = true; - d->base.position_tracking_supported = false; - d->base.device_type = XRT_DEVICE_TYPE_HMD; + vive_setup_trackers(d, tstatus); switch (d->config.variant) { case VIVE_VARIANT_VIVE: snprintf(d->base.str, XRT_DEVICE_NAME_LEN, "HTC Vive (vive)"); break; @@ -896,5 +951,7 @@ vive_device_create(struct os_hid_device *mainboard_dev, return NULL; } + vive_device_setup_ui(d); + return d; } diff --git a/src/xrt/drivers/vive/vive_device.h b/src/xrt/drivers/vive/vive_device.h index 8906f9448..3c194a21e 100644 --- a/src/xrt/drivers/vive/vive_device.h +++ b/src/xrt/drivers/vive/vive_device.h @@ -62,6 +62,8 @@ struct vive_device { bool calibration; bool fusion; + char hand_status[128]; + char slam_status[128]; } gui; struct vive_config config; @@ -77,13 +79,46 @@ struct vive_device //! Prediction struct m_relation_history *relation_hist; } fusion; + + //! Fields related to camera-based tracking (SLAM and hand tracking) + struct + { + //! Set at start. Whether the SLAM tracker was initialized. + bool slam_enabled; + + //! Set at start. Whether the hand tracker was initialized. + bool hand_enabled; + + } tracking; + + //! Whether to track the HMD with 6dof SLAM or fallback to the 3dof tracker + bool slam_over_3dof; +}; + + +/*! + * Summary of the status of various trackers. + * + * @todo Creation flow is a bit broken for now, in the future this info should be closer + * to the tracker creation code, thus avoiding the need to pass it around like this. + */ +struct vive_tracking_status +{ + bool slam_wanted; + bool slam_supported; + bool slam_enabled; + bool hand_wanted; + bool hand_supported; + bool hand_enabled; }; struct vive_device * vive_device_create(struct os_hid_device *mainboard_dev, struct os_hid_device *sensors_dev, struct os_hid_device *watchman_dev, - enum VIVE_VARIANT variant); + enum VIVE_VARIANT variant, + struct vive_tracking_status tstatus); + #ifdef __cplusplus } diff --git a/src/xrt/drivers/vive/vive_prober.c b/src/xrt/drivers/vive/vive_prober.c index 837d91102..696d6ceaa 100644 --- a/src/xrt/drivers/vive/vive_prober.c +++ b/src/xrt/drivers/vive/vive_prober.c @@ -21,16 +21,6 @@ #include "xrt/xrt_config_drivers.h" - -#ifdef XRT_BUILD_DRIVER_HANDTRACKING -#include "../ht/ht_interface.h" -#include "../multi_wrapper/multi.h" -#include "../ht_ctrl_emu/ht_ctrl_emu_interface.h" -DEBUG_GET_ONCE_BOOL_OPTION(vive_use_handtracking, "VIVE_USE_HANDTRACKING", false) -#endif - - - static const char VIVE_PRODUCT_STRING[] = "HTC Vive"; static const char VIVE_PRO_PRODUCT_STRING[] = "VIVE Pro"; static const char VALVE_INDEX_PRODUCT_STRING[] = "Index HMD"; @@ -75,6 +65,7 @@ init_vive1(struct xrt_prober *xp, struct xrt_prober_device **devices, size_t device_count, enum u_logging_level log_level, + struct vive_tracking_status tstatus, struct xrt_device **out_xdev) { log_vive_device(log_level, xp, dev); @@ -128,7 +119,8 @@ init_vive1(struct xrt_prober *xp, free(sensors_dev); return 0; } - struct vive_device *d = vive_device_create(mainboard_dev, sensors_dev, watchman_dev, VIVE_VARIANT_VIVE); + struct vive_device *d = + vive_device_create(mainboard_dev, sensors_dev, watchman_dev, VIVE_VARIANT_VIVE, tstatus); if (d == NULL) { free(sensors_dev); free(mainboard_dev); @@ -146,6 +138,7 @@ init_vive_pro(struct xrt_prober *xp, struct xrt_prober_device **devices, size_t device_count, enum u_logging_level log_level, + struct vive_tracking_status tstatus, struct xrt_device **out_xdev) { XRT_TRACE_MARKER(); @@ -202,7 +195,7 @@ init_vive_pro(struct xrt_prober *xp, free(sensors_dev); return 0; } - struct vive_device *d = vive_device_create(mainboard_dev, sensors_dev, watchman_dev, VIVE_VARIANT_PRO); + struct vive_device *d = vive_device_create(mainboard_dev, sensors_dev, watchman_dev, VIVE_VARIANT_PRO, tstatus); if (d == NULL) { free(sensors_dev); free(mainboard_dev); @@ -220,6 +213,7 @@ init_valve_index(struct xrt_prober *xp, struct xrt_prober_device **devices, size_t device_count, enum u_logging_level log_level, + struct vive_tracking_status tstatus, struct vive_config **out_vive_config, struct xrt_device **out_xdevs) { @@ -258,7 +252,7 @@ init_valve_index(struct xrt_prober *xp, return 0; } - struct vive_device *d = vive_device_create(NULL, sensors_dev, watchman_dev, VIVE_VARIANT_INDEX); + struct vive_device *d = vive_device_create(NULL, sensors_dev, watchman_dev, VIVE_VARIANT_INDEX, tstatus); if (d == NULL) { return 0; } @@ -277,6 +271,7 @@ vive_found(struct xrt_prober *xp, size_t device_count, size_t index, cJSON *attached_data, + struct vive_tracking_status tstatus, struct vive_config **out_vive_config, struct xrt_device **out_xdev) { @@ -294,10 +289,10 @@ vive_found(struct xrt_prober *xp, } switch (dev->product_id) { - case VIVE_PID: return init_vive1(xp, dev, devices, device_count, log_level, out_xdev); - case VIVE_PRO_MAINBOARD_PID: return init_vive_pro(xp, dev, devices, device_count, log_level, out_xdev); + case VIVE_PID: return init_vive1(xp, dev, devices, device_count, log_level, tstatus, out_xdev); + case VIVE_PRO_MAINBOARD_PID: return init_vive_pro(xp, dev, devices, device_count, log_level, tstatus, out_xdev); case VIVE_PRO_LHR_PID: - return init_valve_index(xp, dev, devices, device_count, log_level, out_vive_config, out_xdev); + return init_valve_index(xp, dev, devices, device_count, log_level, tstatus, out_vive_config, out_xdev); default: U_LOG_E("No product ids matched %.4x", dev->product_id); return 0; } diff --git a/src/xrt/drivers/vive/vive_prober.h b/src/xrt/drivers/vive/vive_prober.h index 6d0cf13bb..05ed236a3 100644 --- a/src/xrt/drivers/vive/vive_prober.h +++ b/src/xrt/drivers/vive/vive_prober.h @@ -14,6 +14,8 @@ #include "xrt/xrt_prober.h" +#include "vive_device.h" + #ifdef __cplusplus extern "C" { #endif @@ -51,6 +53,7 @@ vive_found(struct xrt_prober *xp, size_t device_count, size_t index, cJSON *attached_data, + struct vive_tracking_status tstatus, struct vive_config **out_vive_config, struct xrt_device **out_xdev); diff --git a/src/xrt/drivers/wmr/wmr_hmd.c b/src/xrt/drivers/wmr/wmr_hmd.c index 6d6755019..56010368e 100644 --- a/src/xrt/drivers/wmr/wmr_hmd.c +++ b/src/xrt/drivers/wmr/wmr_hmd.c @@ -1594,7 +1594,7 @@ wmr_hmd_setup_trackers(struct wmr_hmd *wh, struct xrt_slam_sinks *out_sinks, str #endif bool hand_enabled = hand_supported && hand_wanted; - wh->base.orientation_tracking_supported = dof3_enabled; + wh->base.orientation_tracking_supported = dof3_enabled || slam_enabled; wh->base.position_tracking_supported = slam_enabled; wh->base.hand_tracking_supported = false; // out_handtracker will handle it diff --git a/src/xrt/targets/common/target_builder_lighthouse.c b/src/xrt/targets/common/target_builder_lighthouse.c index f04845bfc..2fe1fe401 100644 --- a/src/xrt/targets/common/target_builder_lighthouse.c +++ b/src/xrt/targets/common/target_builder_lighthouse.c @@ -9,7 +9,9 @@ */ #include "tracking/t_hand_tracking.h" - +// #include "tracking/t_tracking.h" +// #include "util/u_debug.h" +// #include "v4l2/v4l2_interface.h" #include "xrt/xrt_config_drivers.h" #include "xrt/xrt_device.h" #include "xrt/xrt_prober.h" @@ -31,15 +33,9 @@ #include -//! @todo This should not be in static storage. Maybe make this inherit and replace usysd. -static struct lighthouse_system -{ - bool use_libsurvive; //!< Whether we are using survive driver or vive driver - bool hand_enabled; //!< Whether hand tracking is enabled -} lhs; - #ifdef XRT_BUILD_DRIVER_VIVE #include "vive/vive_prober.h" +#include "vive/vive_device.h" #endif #ifdef XRT_BUILD_DRIVER_SURVIVE @@ -53,8 +49,17 @@ static struct lighthouse_system #include "../../tracking/hand/mercury/hg_interface.h" #endif +//! @todo This should not be in static storage. Maybe make this inherit and replace usysd. +static struct lighthouse_system +{ + bool use_libsurvive; //!< Whether we are using survive driver or vive driver + bool slam_enabled; //!< Whether SLAM tracking is enabled + bool hand_enabled; //!< Whether hand tracking is enabled +} lhs; + DEBUG_GET_ONCE_LOG_OPTION(lh_log, "LH_LOG", U_LOGGING_WARN) DEBUG_GET_ONCE_BOOL_OPTION(vive_over_survive, "VIVE_OVER_SURVIVE", false) +DEBUG_GET_ONCE_BOOL_OPTION(vive_slam, "VIVE_SLAM", true) DEBUG_GET_ONCE_BOOL_OPTION(lh_handtracking, "LH_HANDTRACKING", true) DEBUG_GET_ONCE_BOOL_OPTION(ht_use_old_rgb, "HT_USE_OLD_RGB", false) @@ -321,12 +326,18 @@ setup_visual_trackers(struct u_system_devices *usysd, struct xrt_slam_sinks *out_sinks, struct xrt_device **out_devices) { + bool slam_enabled = lhs.slam_enabled; bool hand_enabled = lhs.hand_enabled; struct t_stereo_camera_calibration *stereo_calib = NULL; struct xrt_pose head_in_left_cam; vive_get_stereo_camera_calibration(hmd_config, &stereo_calib, &head_in_left_cam); + // Initialize SLAM tracker + if (slam_enabled) { + LH_ASSERT(false, "Not implemented"); + } + // Initialize hand tracker struct xrt_slam_sinks *hand_sinks = NULL; struct xrt_device *hand_devices[2] = {NULL}; @@ -392,10 +403,12 @@ stream_data_sources(struct u_system_devices *usysd, struct xrt_prober *xp, struc } bool success = false; - - uint32_t mode = get_selected_mode(finder.xfs); - success = xrt_fs_stream_start(finder.xfs, sinks.left, XRT_FS_CAPTURE_TYPE_TRACKING, mode); - + if (lhs.slam_enabled) { + LH_ASSERT(false, "Not implemented"); + } else { + uint32_t mode = get_selected_mode(finder.xfs); + success = xrt_fs_stream_start(finder.xfs, sinks.left, XRT_FS_CAPTURE_TYPE_TRACKING, mode); + } if (!success) { LH_ERROR("Unable to start data streaming"); @@ -420,6 +433,15 @@ lighthouse_open_system(struct xrt_builder *xb, goto end; } + // Decide whether to initialize the SLAM tracker + bool slam_wanted = debug_get_bool_option_vive_slam(); +#ifdef XRT_FEATURE_SLAM + bool slam_supported = !lhs.use_libsurvive; // Only with vive driver +#else + bool slam_supported = false; +#endif + bool slam_enabled = slam_supported && slam_wanted; + // Decide whether to initialize the hand tracker bool hand_wanted = debug_get_bool_option_lh_handtracking(); #ifdef XRT_BUILD_DRIVER_HANDTRACKING @@ -429,6 +451,7 @@ lighthouse_open_system(struct xrt_builder *xb, #endif bool hand_enabled = hand_supported && hand_wanted; + lhs.slam_enabled = slam_enabled; lhs.hand_enabled = hand_enabled; @@ -440,6 +463,15 @@ lighthouse_open_system(struct xrt_builder *xb, #endif } else { #ifdef XRT_BUILD_DRIVER_VIVE + struct vive_tracking_status tstatus = { + .slam_wanted = slam_wanted, + .slam_supported = slam_supported, + .slam_enabled = slam_enabled, + .hand_wanted = hand_wanted, + .hand_supported = hand_supported, + .hand_enabled = hand_enabled, + }; + struct xrt_prober_device **xpdevs = NULL; size_t xpdev_count = 0; @@ -460,7 +492,7 @@ lighthouse_open_system(struct xrt_builder *xb, case VIVE_PID: case VIVE_PRO_MAINBOARD_PID: case VIVE_PRO_LHR_PID: { - int num_devices = vive_found(xp, xpdevs, xpdev_count, i, NULL, &hmd_config, + int num_devices = vive_found(xp, xpdevs, xpdev_count, i, NULL, tstatus, &hmd_config, &usysd->base.xdevs[usysd->base.xdev_count]); usysd->base.xdev_count += num_devices;