From 53201debdcde4b226b9eadd296cd0ce0e257c07b Mon Sep 17 00:00:00 2001 From: Jakob Bornecrantz Date: Sun, 9 Feb 2020 15:36:16 +0000 Subject: [PATCH] d/rs: Add a simple pure 6DOF device, for the T26[1|5] devices --- CMakeLists.txt | 10 + src/CMakeLists.txt | 4 + src/targets_enabled_drivers.h.cmake_in | 2 + src/xrt/drivers/CMakeLists.txt | 13 ++ src/xrt/drivers/realsense/rs_6dof.c | 283 +++++++++++++++++++++++ src/xrt/drivers/realsense/rs_interface.h | 41 ++++ src/xrt/targets/CMakeLists.txt | 5 + 7 files changed, 358 insertions(+) create mode 100644 src/xrt/drivers/realsense/rs_6dof.c create mode 100644 src/xrt/drivers/realsense/rs_interface.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 43c1238e7..baf8e3112 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,6 +29,7 @@ find_package(OpenHMD) find_package(OpenCV COMPONENTS core calib3d highgui imgproc imgcodecs features2d video) find_package(Libusb1) find_package(JPEG) +find_package(realsense2) find_package(SDL2) find_package(Threads) find_package(ZLIB) @@ -71,6 +72,7 @@ cmake_dependent_option(BUILD_WITH_OPENCV "Enable OpenCV backend" ON "OpenCV_FOUN cmake_dependent_option(BUILD_WITH_LIBUVC "Enable libuvc video driver" ON "LIBUVC_FOUND" OFF) cmake_dependent_option(BUILD_WITH_FFMPEG "Enable ffmpeg testing video driver" ON "FFMPEG_FOUND" OFF) cmake_dependent_option(BUILD_WITH_PSVR "Enable PSVR HMD driver" ON "HIDAPI_FOUND" OFF) +cmake_dependent_option(BUILD_WITH_RS "Enable RealSense device driver" ON "realsense2_FOUND" OFF) option(BUILD_WITH_DUMMY "Enable dummy driver" ON) cmake_dependent_option(BUILD_WITH_VIVE "Enable Vive driver" ON "ZLIB_FOUND" OFF) cmake_dependent_option(BUILD_WITH_OPENHMD "Enable OpenHMD driver" ON "OPENHMD_FOUND" OFF) @@ -152,6 +154,14 @@ if(BUILD_WITH_PSVR) set(BUILD_DRIVER_PSVR TRUE) endif() +if(BUILD_WITH_RS) + if (NOT ${realsense2_FOUND}) + message(FATAL_ERROR "RealSense driver requires librealsense2") + endif() + + set(BUILD_DRIVER_RS TRUE) +endif() + if(BUILD_WITH_VIVE) set(BUILD_DRIVER_VIVE TRUE) endif() diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 25a663ed3..44a735dd2 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -34,6 +34,10 @@ if(BUILD_DRIVER_PSVR) set(XRT_BUILD_DRIVER_PSVR TRUE) endif() +if(BUILD_DRIVER_RS) + set(XRT_BUILD_DRIVER_RS TRUE) +endif() + if(BUILD_DRIVER_V4L2) set(XRT_BUILD_DRIVER_V4L2 TRUE) endif() diff --git a/src/targets_enabled_drivers.h.cmake_in b/src/targets_enabled_drivers.h.cmake_in index 05899445e..521967abb 100644 --- a/src/targets_enabled_drivers.h.cmake_in +++ b/src/targets_enabled_drivers.h.cmake_in @@ -23,6 +23,8 @@ #cmakedefine XRT_BUILD_DRIVER_PSVR +#cmakedefine XRT_BUILD_DRIVER_RS + #cmakedefine XRT_BUILD_DRIVER_V4L2 #cmakedefine XRT_BUILD_DRIVER_VIVE diff --git a/src/xrt/drivers/CMakeLists.txt b/src/xrt/drivers/CMakeLists.txt index e972cc626..892d36dad 100644 --- a/src/xrt/drivers/CMakeLists.txt +++ b/src/xrt/drivers/CMakeLists.txt @@ -106,6 +106,19 @@ if(BUILD_DRIVER_PSVR) list(APPEND ENABLED_HEADSET_DRIVERS psvr) endif() +if(BUILD_DRIVER_RS) + set(RS_SOURCE_FILES + realsense/rs_6dof.c + ) + + # Use OBJECT to not create a archive, since it just gets in the way. + add_library(drv_rs OBJECT ${RS_SOURCE_FILES}) + target_include_directories(drv_rs SYSTEM + PRIVATE ${realsense2_INCLUDE_DIR} + ) + list(APPEND ENABLED_HEADSET_DRIVERS rs) +endif() + if(BUILD_DRIVER_VIVE) set(VIVE_SOURCE_FILES vive/vive_device.h diff --git a/src/xrt/drivers/realsense/rs_6dof.c b/src/xrt/drivers/realsense/rs_6dof.c new file mode 100644 index 000000000..89b049efa --- /dev/null +++ b/src/xrt/drivers/realsense/rs_6dof.c @@ -0,0 +1,283 @@ +// Copyright 2020, Collabora, Ltd. +// Copyright 2020, Nova King. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief RealSense helper driver for 6DOF tracking. + * @author Nova King + * @author Jakob Bornecrantz + * @ingroup drv_rs + */ + +#include "xrt/xrt_defines.h" +#include "xrt/xrt_device.h" + +#include "util/u_device.h" + +#include +#include +#include +#include + +#include +#include +#include + + + +struct rs_6dof +{ + struct xrt_device base; + + struct xrt_pose pose; + + rs2_context *ctx; + rs2_pipeline *pipe; + rs2_pipeline_profile *profile; + rs2_config *config; +}; + + +/*! + * Helper to convert a xdev to a @ref rs_6dof. + */ +static inline struct rs_6dof * +rs_6dof(struct xrt_device *xdev) +{ + return (struct rs_6dof *)xdev; +} + +/*! + * Simple helper to check and print error messages. + */ +static int +check_error(struct rs_6dof *rs, rs2_error *e) +{ + if (e == NULL) { + return 0; + } + + fprintf(stderr, "rs_error was raised when calling %s(%s): \n", + rs2_get_failed_function(e), rs2_get_failed_args(e)); + fprintf(stderr, "%s\n", rs2_get_error_message(e)); + + return 1; +} + +/*! + * Frees all RealSense resources. + */ +static void +close_6dof(struct rs_6dof *rs) +{ + if (rs->config) { + rs2_delete_config(rs->config); + rs->config = NULL; + } + + if (rs->profile) { + rs2_delete_pipeline_profile(rs->profile); + rs->profile = NULL; + } + + if (rs->pipe) { + rs2_pipeline_stop(rs->pipe, NULL); + rs2_delete_pipeline(rs->pipe); + rs->pipe = NULL; + } + + if (rs->ctx) { + rs2_delete_context(rs->ctx); + rs->ctx = NULL; + } +} + +/*! + * Create all RealSense resources needed for 6DOF tracking. + */ +static int +create_6dof(struct rs_6dof *rs) +{ + assert(rs != NULL); + rs2_error *e = NULL; + + rs->ctx = rs2_create_context(RS2_API_VERSION, &e); + if (check_error(rs, e) != 0) { + close_6dof(rs); + return 1; + } + + rs->pipe = rs2_create_pipeline(rs->ctx, &e); + if (check_error(rs, e) != 0) { + close_6dof(rs); + return 1; + } + + rs->config = rs2_create_config(&e); + if (check_error(rs, e) != 0) { + close_6dof(rs); + return 1; + } + + rs2_config_enable_stream(rs->config, RS2_STREAM_POSE, 0, 0, 0, + RS2_FORMAT_6DOF, 200, &e); + if (check_error(rs, e) != 0) { + close_6dof(rs); + return 1; + } + + rs->profile = rs2_pipeline_start_with_config(rs->pipe, rs->config, &e); + if (check_error(rs, e) != 0) { + close_6dof(rs); + return 1; + } + + return 0; +} + +/*! + * Process a frame as 6DOF data, does not assume ownership of the frame. + */ +static void +process_frame(struct rs_6dof *rs, rs2_frame *frame, struct xrt_pose *out_pose) +{ + rs2_error *e = NULL; + int ret = 0; + + ret = rs2_is_frame_extendable_to(frame, RS2_EXTENSION_POSE_FRAME, &e); + if (check_error(rs, e) != 0 || ret == 0) { + return; + } + + rs2_pose camera_pose; + rs2_pose_frame_get_pose_data(frame, &camera_pose, &e); + if (check_error(rs, e) != 0) { + return; + } + + out_pose->orientation.x = camera_pose.rotation.x; + out_pose->orientation.y = camera_pose.rotation.y; + out_pose->orientation.z = camera_pose.rotation.z; + out_pose->orientation.w = camera_pose.rotation.w; + + out_pose->position.x = camera_pose.translation.x; + out_pose->position.y = camera_pose.translation.y; + out_pose->position.z = camera_pose.translation.z; + + rs2_release_frame(frame); +} + +static int +update(struct rs_6dof *rs, struct xrt_pose *out_pose) +{ + rs2_frame *frames; + rs2_error *e = NULL; + + frames = + rs2_pipeline_wait_for_frames(rs->pipe, RS2_DEFAULT_TIMEOUT, &e); + if (check_error(rs, e) != 0) { + return 1; + } + + int num_frames = rs2_embedded_frames_count(frames, &e); + if (check_error(rs, e) != 0) { + return 1; + } + + for (int i = 0; i < num_frames; i++) { + + rs2_frame *frame = rs2_extract_frame(frames, i, &e); + if (check_error(rs, e) != 0) { + rs2_release_frame(frames); + return 1; + } + + // Does not assume ownership of the frame. + process_frame(rs, frame, out_pose); + rs2_release_frame(frame); + + rs2_release_frame(frames); + return 0; + } + + return 0; +} + +static void +rs_6dof_update_inputs(struct xrt_device *xdev, struct time_state *timekeeping) +{ + // Empty +} + +static void +rs_6dof_get_tracked_pose(struct xrt_device *xdev, + enum xrt_input_name name, + struct time_state *timekeeping, + int64_t *out_timestamp, + struct xrt_space_relation *out_relation) +{ + struct rs_6dof *rs = rs_6dof(xdev); + + if (name != XRT_INPUT_GENERIC_HEAD_POSE) { + fprintf(stderr, "unknown input name\n"); + return; + } + + int64_t now = time_state_get_now(timekeeping); + *out_timestamp = now; + + update(rs, &rs->pose); + + out_relation->pose = rs->pose; + out_relation->relation_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); +} + +static void +rs_6dof_get_view_pose(struct xrt_device *xdev, + struct xrt_vec3 *eye_relation, + uint32_t view_index, + struct xrt_pose *out_pose) +{ + assert(false); +} + +static void +rs_6dof_destroy(struct xrt_device *xdev) +{ + struct rs_6dof *rs = rs_6dof(xdev); + close_6dof(rs); + free(rs); +} + +struct xrt_device * +rs_6dof_create(void) +{ + struct rs_6dof *rs = U_DEVICE_ALLOCATE( + struct rs_6dof, U_DEVICE_ALLOC_TRACKING_NONE, 1, 0); + int ret = 0; + + rs->base.update_inputs = rs_6dof_update_inputs; + rs->base.get_tracked_pose = rs_6dof_get_tracked_pose; + rs->base.get_view_pose = rs_6dof_get_view_pose; + rs->base.destroy = rs_6dof_destroy; + rs->base.name = XRT_DEVICE_GENERIC_HMD; // This is a lie. + rs->pose.orientation.w = 1.0f; // All other values set to zero. + + // Print name. + snprintf(rs->base.str, XRT_DEVICE_NAME_LEN, "Intel RealSense 6-DOF"); + + // Setup input, this is a lie. + rs->base.inputs[0].name = XRT_INPUT_GENERIC_HEAD_POSE; + + ret = create_6dof(rs); + if (ret != 0) { + rs_6dof_destroy(&rs->base); + return NULL; + } + + return &rs->base; +} diff --git a/src/xrt/drivers/realsense/rs_interface.h b/src/xrt/drivers/realsense/rs_interface.h new file mode 100644 index 000000000..397fb7907 --- /dev/null +++ b/src/xrt/drivers/realsense/rs_interface.h @@ -0,0 +1,41 @@ +// Copyright 2020, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Interface to RealSense devices. + * @author Jakob Bornecrantz + * @ingroup drv_rs + */ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + + +/*! + * @defgroup drv_rs North Star Driver + * @ingroup drv + * + * @brief Driver for the North Star HMD. + */ + +/*! + * Create a RelaseSense 6DOF tracker device. + * + * @ingroup drv_rs + */ +struct xrt_device * +rs_6dof_create(void); + +/*! + * @dir drivers/realsense + * + * @brief @ref drv_rs files. + */ + + +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/targets/CMakeLists.txt b/src/xrt/targets/CMakeLists.txt index e9eab2f0f..eb4a5069c 100644 --- a/src/xrt/targets/CMakeLists.txt +++ b/src/xrt/targets/CMakeLists.txt @@ -51,6 +51,11 @@ if(BUILD_DRIVER_PSVR) list(APPEND DRIVER_LIBRARIES ${HIDAPI_LIBRARIES}) endif() +if(BUILD_DRIVER_RS) + list(APPEND DRIVER_OBJECTS $) + list(APPEND DRIVER_LIBRARIES ${realsense2_LIBRARY}) +endif() + if(BUILD_DRIVER_V4L2) list(APPEND DRIVER_OBJECTS $) endif()