d/rs: Add a simple pure 6DOF device, for the T26[1|5] devices

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()

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()

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

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

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 <technobaboo@gmail.com>
+ * @author Jakob Bornecrantz <jakob@collabora.com>
+ * @ingroup drv_rs
+ */
+
+#include "xrt/xrt_defines.h"
+#include "xrt/xrt_device.h"
+
+#include "util/u_device.h"
+
+#include <librealsense2/rs.h>
+#include <librealsense2/h/rs_pipeline.h>
+#include <librealsense2/h/rs_option.h>
+#include <librealsense2/h/rs_frame.h>
+
+#include <stdio.h>
+#include <assert.h>
+#include <stdlib.h>
+
+
+
+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;
+}

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 <jakob@collabora.com>
+ * @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

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 $<TARGET_OBJECTS:drv_rs>)
+	list(APPEND DRIVER_LIBRARIES ${realsense2_LIBRARY})
+endif()
+
 if(BUILD_DRIVER_V4L2)
 	list(APPEND DRIVER_OBJECTS $<TARGET_OBJECTS:drv_v4l2>)
 endif()