From 55b86fe815b85cacd40b459177be132d99559afe Mon Sep 17 00:00:00 2001 From: Moses Turner Date: Mon, 29 Mar 2021 17:31:12 -0500 Subject: [PATCH] d/ulv2: Create the driver. Co-authored-by: Moses Turner Co-authored-by: Christoph Haag --- CMakeLists.txt | 16 + .../config_v0.json.northstar_lonestar | 57 ++- meson.build | 9 + src/xrt/drivers/CMakeLists.txt | 9 + src/xrt/drivers/meson.build | 11 + src/xrt/drivers/ultraleap_v2/readme.MD | 12 + src/xrt/drivers/ultraleap_v2/ulv2_driver.cpp | 452 ++++++++++++++++++ src/xrt/drivers/ultraleap_v2/ulv2_interface.h | 35 ++ src/xrt/targets/common/CMakeLists.txt | 4 + src/xrt/targets/common/target_lists.c | 8 + src/xrt/targets/meson.build | 4 + 11 files changed, 599 insertions(+), 18 deletions(-) create mode 100644 src/xrt/drivers/ultraleap_v2/readme.MD create mode 100644 src/xrt/drivers/ultraleap_v2/ulv2_driver.cpp create mode 100644 src/xrt/drivers/ultraleap_v2/ulv2_interface.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 337183880..b2eee630c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -43,6 +43,19 @@ find_package(cJSON) find_package(Systemd) find_package(OpenGLES COMPONENTS V3) +#find_package(Leap) + +find_library(Leap + NAMES libLeap.so + PATHS /usr/local/lib + ) +if (Leap) + include_directories(include /usr/local/include/) + message(STATUS "Found libLeap: ${Leap}") +else() + message(STATUS "Could NOT find libLeap: (missing: /usr/local/lib/libLeap.so)") +endif() + # Android SDK doesn't look for 3.8 and 3.9, which is what new distros ship with. set(Python_ADDITIONAL_VERSIONS 3.8 3.9) if(NOT CMAKE_VERSION VERSION_LESS 3.12) @@ -185,6 +198,7 @@ cmake_dependent_option(XRT_BUILD_DRIVER_ARDUINO "Enable Arduino input device wit cmake_dependent_option(XRT_BUILD_DRIVER_ILLIXR "Enable ILLIXR driver" ON "ILLIXR_PATH" OFF) option(XRT_BUILD_DRIVER_DUMMY "Enable dummy driver" ON) +cmake_dependent_option(XRT_BUILD_DRIVER_ULV2 "Enable Ultraleap v2 driver" ON "Leap" OFF) cmake_dependent_option(XRT_BUILD_DRIVER_REMOTE "Enable remote debugging driver" ON "XRT_HAVE_LINUX OR ANDROID" OFF) # These all use the Monado internal hid wrapper. @@ -219,6 +233,7 @@ list(APPEND AVAILABLE_DRIVERS "REMOTE" "SURVIVE" "V4L2" + "ULV2" "VF" "VIVE" "QWERTY" @@ -350,6 +365,7 @@ message(STATUS "# DRIVER_HDK: ${XRT_BUILD_DRIVER_HDK}") message(STATUS "# DRIVER_HYDRA: ${XRT_BUILD_DRIVER_HYDRA}") message(STATUS "# DRIVER_ILLIXR: ${XRT_BUILD_DRIVER_ILLIXR}") message(STATUS "# DRIVER_NS: ${XRT_BUILD_DRIVER_NS}") +message(STATUS "# DRIVER_ULV2: ${XRT_BUILD_DRIVER_ULV2}") message(STATUS "# DRIVER_OHMD: ${XRT_BUILD_DRIVER_OHMD}") message(STATUS "# DRIVER_PSMV: ${XRT_BUILD_DRIVER_PSMV}") message(STATUS "# DRIVER_PSVR: ${XRT_BUILD_DRIVER_PSVR}") diff --git a/doc/example_configs/config_v0.json.northstar_lonestar b/doc/example_configs/config_v0.json.northstar_lonestar index 0d422a762..b33710f75 100644 --- a/doc/example_configs/config_v0.json.northstar_lonestar +++ b/doc/example_configs/config_v0.json.northstar_lonestar @@ -1,23 +1,44 @@ { - "active": "tracking", - "tracking": { - "tracking_overrides": [{ - "target_device_serial": "North Star", - "tracker_device_serial": "Intel RealSense 6-DOF", - "offset": { - "orientation": { - "x": 0, - "y": -0.068300001323223114, - "z": 0.074400000274181366, - "w": 0.99468898773193359 + "active": "tracking", + "tracking": { + "tracking_overrides": [ + { + "target_device_serial": "North Star", + "tracker_device_serial": "Intel RealSense 6-DOF", + "type": "direct", + "offset": { + "orientation": { + "x": -0.102931, + "y": 0, + "z": 0, + "w": 0.994689 }, - "position": { - "x": 0, - "y": -0.068300001323223114, - "z": 0.074400000274181366 + "position": { + "x": 0, + "y": 0.0683, + "z": -0.0744 } } - }], - "version": 0 + }, + { + "target_device_serial": "Leap Motion v2 driver", + "tracker_device_serial": "Intel RealSense 6-DOF", + "type": "attached", + "offset": { + "orientation": { + "x": 0, + "y": 0, + "z": 0, + "w": 1 + }, + "position": { + "x": 0, + "y": 0.005, + "z": 0 + } + } + } + ], + "version": 0 } -} +} \ No newline at end of file diff --git a/meson.build b/meson.build index 11b31f8ff..c65b9f091 100644 --- a/meson.build +++ b/meson.build @@ -78,6 +78,9 @@ gst_video= dependency('gstreamer-video-1.0', required: false) gst_found = gst.found() and gst_app.found() and gst_video.found() +leap = cc.find_library('Leap', dirs : ['/usr/local/lib'], required: false) +inc_leap = include_directories('/usr/local/include') + opencv = dependency('opencv4', required: false) if not opencv.found() opencv = dependency('opencv', required: get_option('tracking')) @@ -207,6 +210,12 @@ if gst_found and ('auto' in drivers or 'vf' in drivers) endif endif +if leap.found() and ('auto' in drivers or 'ulv2' in drivers) + if 'ulv2' not in drivers + drivers += ['ulv2'] + endif +endif + if survive.found() and ('auto' in drivers and 'survive' not in drivers) drivers += ['survive'] endif diff --git a/src/xrt/drivers/CMakeLists.txt b/src/xrt/drivers/CMakeLists.txt index d0e3033cc..913e4b0b9 100644 --- a/src/xrt/drivers/CMakeLists.txt +++ b/src/xrt/drivers/CMakeLists.txt @@ -101,6 +101,15 @@ if(XRT_BUILD_DRIVER_NS) list(APPEND ENABLED_HEADSET_DRIVERS ns) endif() +if(XRT_BUILD_DRIVER_ULV2) + set(ULV2_SOURCE_FILES + ultraleap_v2/ulv2_driver.cpp + ultraleap_v2/ulv2_interface.h + ) + add_library(drv_ulv2 STATIC ${ULV2_SOURCE_FILES}) + target_link_libraries(drv_ulv2 PRIVATE xrt-interfaces aux_util aux_math Leap) +endif() + if(XRT_BUILD_DRIVER_OHMD) set(OHMD_SOURCE_FILES ohmd/oh_device.c diff --git a/src/xrt/drivers/meson.build b/src/xrt/drivers/meson.build index 18b6a6a87..fba34c8ef 100644 --- a/src/xrt/drivers/meson.build +++ b/src/xrt/drivers/meson.build @@ -58,6 +58,17 @@ lib_drv_ns = static_library( build_by_default: 'ns' in drivers, ) +lib_drv_ulv2 = static_library( + 'drv_ulv2', + files( + 'ultraleap_v2/ulv2_driver.cpp', + 'ultraleap_v2/ulv2_interface.h', + ), + include_directories: [xrt_include, inc_leap], + dependencies: [aux, leap], + build_by_default: 'ulv2' in drivers, +) + lib_drv_ht = static_library( 'drv_ht', files( diff --git a/src/xrt/drivers/ultraleap_v2/readme.MD b/src/xrt/drivers/ultraleap_v2/readme.MD new file mode 100644 index 000000000..692d4ac02 --- /dev/null +++ b/src/xrt/drivers/ultraleap_v2/readme.MD @@ -0,0 +1,12 @@ +## Building +To build you need `Leap.h` and `LeapMath.h` in `/usr/local/include`; and `libLeap.so` in `/usr/local/lib`, and this should automatically build. + +## Running +To have the ultraleap driver successfully initialize, you need to have the Leap Motion Controller plugged in, and `leapd` running. Running `sudo leapd` in another terminal works but it may be slightly more convenient to have it run as a systemd service. + +## Configuring +Presumably, you're using this driver because you want to stick the Leap Motion Controller on the front of your HMD and have it track your hands. + +If you don't have a config file at ~/.config/monado/config_v0.json (or wherever you set `XDG_CONFIG_DIR`), your tracked hands will show up near the tracking origin and not move around with your HMD, which is probably not what you want. + +Instead you probably want to configure Monado to make your Leap Motion Controller-tracked hands follow around your HMD. There's an example of how to do this with North Star in `doc/example_configs/config_v0.json.northstar_lonestar`. If you're using a North Star headset, that should work but unless you're using the Lone Star NS variant you'll need to edit the offsets. If you're using some other HMD you'll have to edit the `tracker_device_serial` to be your HMD serial, and your own offsets. \ No newline at end of file diff --git a/src/xrt/drivers/ultraleap_v2/ulv2_driver.cpp b/src/xrt/drivers/ultraleap_v2/ulv2_driver.cpp new file mode 100644 index 000000000..d05891c68 --- /dev/null +++ b/src/xrt/drivers/ultraleap_v2/ulv2_driver.cpp @@ -0,0 +1,452 @@ +// Copyright 2020-2021, Collabora, Ltd. +// Copyright 2020-2021, Moses Turner +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Driver for Ultraleap's V2 API for the Leap Motion Controller. + * @author Moses Turner + * @author Christoph Haag + * @ingroup drv_ulv2 + */ + +#include "ulv2_interface.h" +#include "util/u_device.h" +#include "util/u_var.h" +#include "util/u_debug.h" +#include "math/m_space.h" +#include "math/m_api.h" +#include "util/u_time.h" +#include "os/os_time.h" +#include "os/os_threading.h" + +#include "Leap.h" + +DEBUG_GET_ONCE_LOG_OPTION(ulv2_log, "ULV2_LOG", U_LOGGING_INFO) + +#define ULV2_TRACE(ulv2d, ...) U_LOG_XDEV_IFL_T(&ulv2d->base, ulv2d->ll, __VA_ARGS__) +#define ULV2_DEBUG(ulv2d, ...) U_LOG_XDEV_IFL_D(&ulv2d->base, ulv2d->ll, __VA_ARGS__) +#define ULV2_INFO(ulv2d, ...) U_LOG_XDEV_IFL_I(&ulv2d->base, ulv2d->ll, __VA_ARGS__) +#define ULV2_WARN(ulv2d, ...) U_LOG_XDEV_IFL_W(&ulv2d->base, ulv2d->ll, __VA_ARGS__) +#define ULV2_ERROR(ulv2d, ...) U_LOG_XDEV_IFL_E(&ulv2d->base, ulv2d->ll, __VA_ARGS__) + +#define printf_pose(pose) \ + printf("%f %f %f %f %f %f %f\n", pose.position.x, pose.position.y, pose.position.z, pose.orientation.x, \ + pose.orientation.y, pose.orientation.z, pose.orientation.w); +extern "C" { + +enum xrt_space_relation_flags valid_flags = (enum xrt_space_relation_flags)( + XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | + XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT); + +// Excuse my sketchy thread stuff, I'm sure this violates all kinds of best practices. It uusssuallyyy doesn't explode. +// -Moses Turner +enum leap_thread_status +{ + THREAD_NOT_STARTED, + THREAD_OK, + THREAD_ERRORED_OUT, +}; + +struct ulv2_device +{ + struct xrt_device base; + + struct xrt_tracking_origin tracking_origin; + + enum u_logging_level ll; + + bool pthread_should_stop; + + enum leap_thread_status our_thread_status; + + struct os_thread_helper leap_loop_oth; + + struct xrt_hand_joint_set joints_write_in[2]; + + // Only read/write these last two if you have the mutex + struct xrt_hand_joint_set joints_read_out[2]; + + bool hand_exists[2]; +}; + +inline struct ulv2_device * +ulv2_device(struct xrt_device *xdev) +{ + return (struct ulv2_device *)xdev; +} + + +// Roughly, take the Leap hand joint, do some coordinate conversions, and save it in a xrt_hand_joint_value +static void +ulv2_process_joint( + Leap::Vector jointpos, Leap::Matrix jointbasis, float width, int side, struct xrt_hand_joint_value *joint) +{ + struct xrt_space_relation *relation = &joint->relation; + joint->radius = (width / 1000) / 2; + + struct xrt_matrix_3x3 turn_into_quat; + // clang-format off + // These are matrices so we want to preserve where the rows and columns are, hence the clang-format off + if (side == 1) + { + turn_into_quat = {-jointbasis.xBasis.x, -jointbasis.yBasis.x, -jointbasis.zBasis.x, + -jointbasis.xBasis.z, -jointbasis.yBasis.z, -jointbasis.zBasis.z, + -jointbasis.xBasis.y, -jointbasis.yBasis.y, -jointbasis.zBasis.y}; + } + else + { + turn_into_quat = {jointbasis.xBasis.x, -jointbasis.yBasis.x, -jointbasis.zBasis.x, + jointbasis.xBasis.z, -jointbasis.yBasis.z, -jointbasis.zBasis.z, + jointbasis.xBasis.y, -jointbasis.yBasis.y, -jointbasis.zBasis.y}; + } + // clang-format on + + + math_quat_from_matrix_3x3(&turn_into_quat, &relation->pose.orientation); + relation->pose.position.x = jointpos.x * -1 / 1000.0; + relation->pose.position.y = jointpos.z * -1 / 1000.0; + relation->pose.position.z = jointpos.y * -1 / 1000.0; + relation->relation_flags = valid_flags; +} + +static int error_time; // Lazy counter to prevent printing error messages at 120hz + + +void +ulv2_process_hand(Leap::Hand hand, xrt_hand_joint_set *joint_set, int hi) +{ + +#define xrtj(y) &joint_set->values.hand_joint_set_default[XRT_HAND_JOINT_##y] + + ulv2_process_joint(hand.palmPosition(), hand.basis(), 50, hi, xrtj(PALM)); + ulv2_process_joint(hand.wristPosition(), hand.arm().basis(), 50, hi, xrtj(WRIST)); + + const Leap::FingerList fingers = hand.fingers(); + + // Bunch of macros to make the following + // boilerplate easier to deal with + +#define fb(y) finger.bone(y) +#define prevJ(y) finger.bone(y).prevJoint() +#define nextJ(y) finger.bone(y).nextJoint() + +#define lm Leap::Bone::TYPE_METACARPAL +#define lp Leap::Bone::TYPE_PROXIMAL +#define li Leap::Bone::TYPE_INTERMEDIATE +#define ld Leap::Bone::TYPE_DISTAL + + for (Leap::FingerList::const_iterator fl = fingers.begin(); fl != fingers.end(); ++fl) { + // Iterate on the list of fingers Leap gives us + const Leap::Finger finger = *fl; + Leap::Finger::Type fingerType = finger.type(); + switch (fingerType) { + case Leap::Finger::Type::TYPE_THUMB: + // If the finger is a thumb, then for each joint: process the Leap joint location, + // rotation matrix, finger width, hand side (0 if left, 1 if right), + // and write the finger radius and powe out to the correct xrt_hand_joint_value. + ulv2_process_joint(prevJ(lp), fb(lp).basis(), fb(lp).width(), hi, xrtj(THUMB_METACARPAL)); + ulv2_process_joint(prevJ(li), fb(li).basis(), fb(lp).width(), hi, xrtj(THUMB_PROXIMAL)); + ulv2_process_joint(prevJ(ld), fb(ld).basis(), fb(li).width(), hi, xrtj(THUMB_DISTAL)); + ulv2_process_joint(nextJ(ld), fb(ld).basis(), fb(ld).width(), hi, xrtj(THUMB_TIP)); + // Note that there are only four joints here as opposed to all the other fingers which have five + // joints. + break; + case Leap::Finger::Type::TYPE_INDEX: + // If the finger is an index finger, do the same thing but with index joints + ulv2_process_joint(prevJ(lm), fb(lm).basis(), fb(lm).width(), hi, xrtj(INDEX_METACARPAL)); + ulv2_process_joint(prevJ(lp), fb(lp).basis(), fb(lm).width(), hi, xrtj(INDEX_PROXIMAL)); + ulv2_process_joint(prevJ(li), fb(li).basis(), fb(lp).width(), hi, xrtj(INDEX_INTERMEDIATE)); + ulv2_process_joint(prevJ(ld), fb(ld).basis(), fb(li).width(), hi, xrtj(INDEX_DISTAL)); + ulv2_process_joint(nextJ(ld), fb(ld).basis(), fb(ld).width(), hi, xrtj(INDEX_TIP)); + break; + case Leap::Finger::Type::TYPE_MIDDLE: + // If the finger is a middle finger, do the same thing but with middle joints + ulv2_process_joint(prevJ(lm), fb(lm).basis(), fb(lm).width(), hi, xrtj(MIDDLE_METACARPAL)); + ulv2_process_joint(prevJ(lp), fb(lp).basis(), fb(lm).width(), hi, xrtj(MIDDLE_PROXIMAL)); + ulv2_process_joint(prevJ(li), fb(li).basis(), fb(lp).width(), hi, xrtj(MIDDLE_INTERMEDIATE)); + ulv2_process_joint(prevJ(ld), fb(ld).basis(), fb(li).width(), hi, xrtj(MIDDLE_DISTAL)); + ulv2_process_joint(nextJ(ld), fb(ld).basis(), fb(ld).width(), hi, xrtj(MIDDLE_TIP)); + break; + case Leap::Finger::Type::TYPE_RING: + // Ad nauseum. + ulv2_process_joint(prevJ(lm), fb(lm).basis(), fb(lm).width(), hi, xrtj(RING_METACARPAL)); + ulv2_process_joint(prevJ(lp), fb(lp).basis(), fb(lm).width(), hi, xrtj(RING_PROXIMAL)); + ulv2_process_joint(prevJ(li), fb(li).basis(), fb(lp).width(), hi, xrtj(RING_INTERMEDIATE)); + ulv2_process_joint(prevJ(ld), fb(ld).basis(), fb(li).width(), hi, xrtj(RING_DISTAL)); + ulv2_process_joint(nextJ(ld), fb(ld).basis(), fb(ld).width(), hi, xrtj(RING_TIP)); + break; + case Leap::Finger::Type::TYPE_PINKY: + ulv2_process_joint(prevJ(lm), fb(lm).basis(), fb(lm).width(), hi, xrtj(LITTLE_METACARPAL)); + ulv2_process_joint(prevJ(lp), fb(lp).basis(), fb(lm).width(), hi, xrtj(LITTLE_PROXIMAL)); + ulv2_process_joint(prevJ(li), fb(li).basis(), fb(lp).width(), hi, xrtj(LITTLE_INTERMEDIATE)); + ulv2_process_joint(prevJ(ld), fb(ld).basis(), fb(li).width(), hi, xrtj(LITTLE_DISTAL)); + ulv2_process_joint(nextJ(ld), fb(ld).basis(), fb(ld).width(), hi, xrtj(LITTLE_TIP)); + break; + // I hear that Sagittarius has a better api, in C even, so hopefully + // there'll be less weird boilerplate whenever we get access to that + } + } +} + +void * +leap_input_loop(void *ptr_to_xdev) +{ + float retry_sleep_time = 0.05; + float timeout = 0.5; + int num_tries = (timeout / retry_sleep_time); + bool succeeded_connected = false; + bool succeeded_service_connected = false; + + struct xrt_device *xdev = (struct xrt_device *)ptr_to_xdev; + struct ulv2_device *ulv2d = ulv2_device(xdev); + ULV2_DEBUG(ulv2d, "num tries %d; sleep time %f", num_tries, timeout); + + Leap::Controller LeapController; + os_nanosleep(U_1_000_000_000 * 0.01); + // sleep for an arbitrary amount of time so that Leap::Controller can initialize and connect to the service. + float start = (float)os_monotonic_get_ns() / (float)U_1_000_000_000; + // would be nice to do this only if we're not building release ^^. don't know how to do that though. + + for (int i = 0; i < num_tries; i++) { + succeeded_connected = LeapController.isConnected(); + succeeded_service_connected = LeapController.isServiceConnected(); + if (succeeded_connected) { + ULV2_INFO(ulv2d, "Leap Motion Controller connected!"); + break; + } + if (succeeded_service_connected) { + ULV2_INFO(ulv2d, + "Connected to Leap service, but not " + "connected to Leap Motion " + "controller. Retrying (%i / %i)", + i, num_tries); + // This codepath should very rarely get hit as nowadays this gets probed by VID/PID, so you'd + // have to be pretty fast to unplug after it gets probed and before this check. + } else { + ULV2_INFO(ulv2d, + "Not connected to Leap service. " + "Retrying (%i / %i)", + i, num_tries); + } + os_nanosleep(U_1_000_000_000 * retry_sleep_time); // 1 second * retry_sleep_time + } + + ULV2_DEBUG(ulv2d, "Waited %f seconds", ((float)os_monotonic_get_ns() / (float)U_1_000_000_000) - start); + + bool hmdpolicyset = false; + + + if (!succeeded_connected) { + if (succeeded_service_connected) { + ULV2_INFO(ulv2d, + "Connected to Leap service, but couldn't " + "connect to leap motion controller.\n" + "Is it plugged in and has your Leap service " + "detected it?"); + // ditto on this codepath + } else { + ULV2_INFO(ulv2d, + "Couldn't connect to Leap service. Try " + "running sudo leapd in another terminal."); + } + goto cleanup_leap_loop; + } + + // Try to let the Leap service know that we are on a HMD, not on a desk. + for (int i = 0; i < num_tries; i++) { + LeapController.setPolicy(Leap::Controller::POLICY_OPTIMIZE_HMD); + os_nanosleep(time_s_to_ns(0.02)); + LeapController.setPolicy(Leap::Controller::POLICY_OPTIMIZE_HMD); + hmdpolicyset = LeapController.isPolicySet(Leap::Controller::POLICY_OPTIMIZE_HMD); + if (!hmdpolicyset) { + ULV2_ERROR(ulv2d, "Couldn't set HMD policy. Retrying (%i / %i)", i, num_tries); + } else { + ULV2_DEBUG(ulv2d, "HMD policy set."); + break; + } + os_nanosleep(U_1_000_000_000 * retry_sleep_time); // 1 second * retry_sleep_time + } + ULV2_TRACE(ulv2d, "thread OK\n"); + ulv2d->our_thread_status = THREAD_OK; + + + // Main loop + while (!ulv2d->pthread_should_stop) { + + if (!LeapController.isConnected()) { + if ((error_time % 100) == 0) + ULV2_ERROR(ulv2d, "LeapController is not connected\n"); + os_nanosleep(U_1_000_000_000 * 0.1); + error_time += 1; + continue; + } + error_time = 100; // if there's an error next time, the modulo will return 0. + + Leap::Frame frame = LeapController.frame(); + Leap::HandList hands = frame.hands(); + bool leftbeendone = false; + bool rightbeendone = false; + for (Leap::HandList::const_iterator hl = hands.begin(); hl != hands.end(); ++hl) { + int hi; // hand index + const Leap::Hand hand = *hl; + // if (hand.confidence() < *hand_min_confidence) + // continue; + if (hand.isLeft()) { + if (leftbeendone) + continue; // in case there are more than one left hand + leftbeendone = true; + hi = 0; + } + if (!hand.isLeft()) { + if (rightbeendone) + continue; // in case there are more than one right hand + rightbeendone = true; + hi = 1; + } + + ulv2_process_hand(hand, &ulv2d->joints_write_in[hi], hi); + } + os_thread_helper_lock(&ulv2d->leap_loop_oth); + memcpy(&ulv2d->joints_read_out, &ulv2d->joints_write_in, sizeof(struct xrt_hand_joint_set) * 2); + //! @todo (Moses Turner) Could be using LeapController.now() to try to emulate our own pose prediction, + //! but I ain't got time for that + ulv2d->hand_exists[0] = leftbeendone; + ulv2d->hand_exists[1] = rightbeendone; + os_thread_helper_unlock(&ulv2d->leap_loop_oth); + } + +cleanup_leap_loop: + ULV2_TRACE(ulv2d, "leaving input thread\n"); + ulv2d->our_thread_status = THREAD_ERRORED_OUT; + pthread_exit(NULL); +} + +static void +ulv2_device_update_inputs(struct xrt_device *xdev) +{ + // Empty +} + + +static void +ulv2_device_get_hand_tracking(struct xrt_device *xdev, + enum xrt_input_name name, + uint64_t at_timestamp_ns, + struct xrt_hand_joint_set *out_value) +{ + + //! @todo this function doesn't do anything with at_timestamp_ns, + // would be nice to add pose-prediction. Probably once leap motion sagittarius comes out for Linux + + struct ulv2_device *ulv2d = ulv2_device(xdev); + + if (name != XRT_INPUT_GENERIC_HAND_TRACKING_LEFT && name != XRT_INPUT_GENERIC_HAND_TRACKING_RIGHT) { + ULV2_ERROR(ulv2d, "unknown input name for hand tracker"); + return; + } + + bool hand_index = (name == XRT_INPUT_GENERIC_HAND_TRACKING_RIGHT); // 0 if left, 1 if right. + + bool hand_valid = ulv2d->hand_exists[hand_index]; + + os_thread_helper_lock(&ulv2d->leap_loop_oth); + memcpy(out_value, &ulv2d->joints_read_out[hand_index], sizeof(struct xrt_hand_joint_set)); + hand_valid = ulv2d->hand_exists[hand_index]; + os_thread_helper_unlock(&ulv2d->leap_loop_oth); + m_space_relation_ident(&out_value->hand_pose); + + if (hand_valid) { + out_value->is_active = true; + out_value->hand_pose.relation_flags = valid_flags; + } else { + out_value->is_active = false; + } +} + +static void +ulv2_device_destroy(struct xrt_device *xdev) +{ + struct ulv2_device *ulv2d = ulv2_device(xdev); + + ulv2d->pthread_should_stop = true; + os_thread_helper_stop(&ulv2d->leap_loop_oth); + + // Remove the variable tracking. + u_var_remove_root(ulv2d); + + u_device_free(&ulv2d->base); +} + +int +ulv2_found(struct xrt_prober *xp, + struct xrt_prober_device **devices, + size_t num_devices, + size_t index, + cJSON *attached_data, + struct xrt_device **out_xdev) +{ + enum u_device_alloc_flags flags = U_DEVICE_ALLOC_NO_FLAGS; + + int num_hands = 2; + + struct ulv2_device *ulv2d = U_DEVICE_ALLOCATE(struct ulv2_device, flags, num_hands, 0); + + os_thread_helper_init(&ulv2d->leap_loop_oth); + os_thread_helper_start(&ulv2d->leap_loop_oth, (&leap_input_loop), (void *)&ulv2d->base); + + ulv2d->base.tracking_origin = &ulv2d->tracking_origin; + ulv2d->base.tracking_origin->type = XRT_TRACKING_TYPE_OTHER; + + math_pose_identity(&ulv2d->base.tracking_origin->offset); + + ulv2d->ll = debug_get_log_option_ulv2_log(); + + ulv2d->base.update_inputs = ulv2_device_update_inputs; + ulv2d->base.get_hand_tracking = ulv2_device_get_hand_tracking; + ulv2d->base.destroy = ulv2_device_destroy; + + strncpy(ulv2d->base.str, "Leap Motion v2 driver", XRT_DEVICE_NAME_LEN); + strncpy(ulv2d->base.serial, "Leap Motion v2 driver", XRT_DEVICE_NAME_LEN); + + ulv2d->base.inputs[0].name = XRT_INPUT_GENERIC_HAND_TRACKING_LEFT; + ulv2d->base.inputs[1].name = XRT_INPUT_GENERIC_HAND_TRACKING_RIGHT; + + ulv2d->base.name = XRT_DEVICE_HAND_TRACKER; + + ulv2d->base.device_type = XRT_DEVICE_TYPE_HAND_TRACKER; + ulv2d->base.hand_tracking_supported = true; + + u_var_add_root(ulv2d, "Leap Motion v2 driver", true); + u_var_add_ro_text(ulv2d, ulv2d->base.str, "Name"); + + + + uint64_t start_time = os_monotonic_get_ns(); + uint64_t too_long = time_s_to_ns(15.0f); + + while (ulv2d->our_thread_status != THREAD_OK) { + ULV2_TRACE(ulv2d, "waiting... thread status is %d", ulv2d->our_thread_status); + if (ulv2d->our_thread_status == THREAD_ERRORED_OUT) + goto cleanup; + if ((os_monotonic_get_ns() - (uint64_t)start_time) > too_long) { + ULV2_ERROR(ulv2d, + "For some reason the Leap thread locked up. This is a serious error and should " + "never happen."); + goto cleanup; + } + os_nanosleep(time_s_to_ns(0.01)); + } + + + ULV2_INFO(ulv2d, "Hand Tracker initialized!"); + + out_xdev[0] = &ulv2d->base; + + return 1; + +cleanup: + ulv2_device_destroy(&ulv2d->base); + return 0; +} + +} // extern "C" diff --git a/src/xrt/drivers/ultraleap_v2/ulv2_interface.h b/src/xrt/drivers/ultraleap_v2/ulv2_interface.h new file mode 100644 index 000000000..40c5843db --- /dev/null +++ b/src/xrt/drivers/ultraleap_v2/ulv2_interface.h @@ -0,0 +1,35 @@ +// Copyright 2020-2021, Collabora, Ltd. +// Copyright 2020-2021, Moses Turner. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Driver for Ultraleap's V2 API for the Leap Motion Controller. + * @author Moses Turner + * @author Christoph Haag + * @ingroup drv_ulv2 + */ + +#pragma once + +#include "math/m_api.h" +#include "xrt/xrt_device.h" +#include "xrt/xrt_prober.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#define ULV2_VID 0xf182 +#define ULV2_PID 0x0003 + +int +ulv2_found(struct xrt_prober *xp, + struct xrt_prober_device **devices, + size_t num_devices, + size_t index, + cJSON *attached_data, + struct xrt_device **out_xdev); + +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/targets/common/CMakeLists.txt b/src/xrt/targets/common/CMakeLists.txt index 30d048686..9095da43e 100644 --- a/src/xrt/targets/common/CMakeLists.txt +++ b/src/xrt/targets/common/CMakeLists.txt @@ -44,6 +44,10 @@ if(XRT_BUILD_DRIVER_NS) target_link_libraries(target_lists PRIVATE drv_ns) endif() +if(XRT_BUILD_DRIVER_ULV2) + target_link_libraries(target_lists PRIVATE drv_ulv2) +endif() + if(XRT_BUILD_DRIVER_OHMD) target_link_libraries(target_lists PRIVATE drv_ohmd) endif() diff --git a/src/xrt/targets/common/target_lists.c b/src/xrt/targets/common/target_lists.c index 8b51cdbd8..4413fbe62 100644 --- a/src/xrt/targets/common/target_lists.c +++ b/src/xrt/targets/common/target_lists.c @@ -70,6 +70,10 @@ #include "realsense/rs_interface.h" #endif +#ifdef XRT_BUILD_DRIVER_ULV2 +#include "ultraleap_v2/ulv2_interface.h" +#endif + #ifdef XRT_BUILD_DRIVER_QWERTY #include "qwerty/qwerty_interface.h" #endif @@ -113,6 +117,10 @@ struct xrt_prober_entry target_entry_list[] = { {VALVE_VID, VIVE_WATCHMAN_DONGLE_GEN2, vive_controller_found, "Valve Watchman Wireless Device", "vive"}, #endif +#ifdef XRT_BUILD_DRIVER_ULV2 + {ULV2_VID, ULV2_PID, ulv2_found, "Leap Motion Controller", "ulv2"}, +#endif + {0x0000, 0x0000, NULL, NULL, NULL}, // Terminate }; diff --git a/src/xrt/targets/meson.build b/src/xrt/targets/meson.build index d3ebd476f..9ac834703 100644 --- a/src/xrt/targets/meson.build +++ b/src/xrt/targets/meson.build @@ -81,6 +81,10 @@ if 'remote' in drivers driver_libs += [lib_drv_remote] endif +if 'ulv2' in drivers + driver_libs += [lib_drv_ulv2] +endif + driver_libs += [lib_drv_multi] subdir('common')