The open source OpenXR runtime
d/rs: Add a simple pure 6DOF device, for the T26[1|5] devices
+358
+10
CMakeLists.txt
+10
CMakeLists.txt
···
29
29
find_package(OpenCV COMPONENTS core calib3d highgui imgproc imgcodecs features2d video)
30
30
find_package(Libusb1)
31
31
find_package(JPEG)
32
+
find_package(realsense2)
32
33
find_package(SDL2)
33
34
find_package(Threads)
34
35
find_package(ZLIB)
···
71
72
cmake_dependent_option(BUILD_WITH_LIBUVC "Enable libuvc video driver" ON "LIBUVC_FOUND" OFF)
72
73
cmake_dependent_option(BUILD_WITH_FFMPEG "Enable ffmpeg testing video driver" ON "FFMPEG_FOUND" OFF)
73
74
cmake_dependent_option(BUILD_WITH_PSVR "Enable PSVR HMD driver" ON "HIDAPI_FOUND" OFF)
75
+
cmake_dependent_option(BUILD_WITH_RS "Enable RealSense device driver" ON "realsense2_FOUND" OFF)
74
76
option(BUILD_WITH_DUMMY "Enable dummy driver" ON)
75
77
cmake_dependent_option(BUILD_WITH_VIVE "Enable Vive driver" ON "ZLIB_FOUND" OFF)
76
78
cmake_dependent_option(BUILD_WITH_OPENHMD "Enable OpenHMD driver" ON "OPENHMD_FOUND" OFF)
···
150
152
endif()
151
153
152
154
set(BUILD_DRIVER_PSVR TRUE)
155
+
endif()
156
+
157
+
if(BUILD_WITH_RS)
158
+
if (NOT ${realsense2_FOUND})
159
+
message(FATAL_ERROR "RealSense driver requires librealsense2")
160
+
endif()
161
+
162
+
set(BUILD_DRIVER_RS TRUE)
153
163
endif()
154
164
155
165
if(BUILD_WITH_VIVE)
+4
src/CMakeLists.txt
+4
src/CMakeLists.txt
+2
src/targets_enabled_drivers.h.cmake_in
+2
src/targets_enabled_drivers.h.cmake_in
+13
src/xrt/drivers/CMakeLists.txt
+13
src/xrt/drivers/CMakeLists.txt
···
106
106
list(APPEND ENABLED_HEADSET_DRIVERS psvr)
107
107
endif()
108
108
109
+
if(BUILD_DRIVER_RS)
110
+
set(RS_SOURCE_FILES
111
+
realsense/rs_6dof.c
112
+
)
113
+
114
+
# Use OBJECT to not create a archive, since it just gets in the way.
115
+
add_library(drv_rs OBJECT ${RS_SOURCE_FILES})
116
+
target_include_directories(drv_rs SYSTEM
117
+
PRIVATE ${realsense2_INCLUDE_DIR}
118
+
)
119
+
list(APPEND ENABLED_HEADSET_DRIVERS rs)
120
+
endif()
121
+
109
122
if(BUILD_DRIVER_VIVE)
110
123
set(VIVE_SOURCE_FILES
111
124
vive/vive_device.h
+283
src/xrt/drivers/realsense/rs_6dof.c
+283
src/xrt/drivers/realsense/rs_6dof.c
···
1
+
// Copyright 2020, Collabora, Ltd.
2
+
// Copyright 2020, Nova King.
3
+
// SPDX-License-Identifier: BSL-1.0
4
+
/*!
5
+
* @file
6
+
* @brief RealSense helper driver for 6DOF tracking.
7
+
* @author Nova King <technobaboo@gmail.com>
8
+
* @author Jakob Bornecrantz <jakob@collabora.com>
9
+
* @ingroup drv_rs
10
+
*/
11
+
12
+
#include "xrt/xrt_defines.h"
13
+
#include "xrt/xrt_device.h"
14
+
15
+
#include "util/u_device.h"
16
+
17
+
#include <librealsense2/rs.h>
18
+
#include <librealsense2/h/rs_pipeline.h>
19
+
#include <librealsense2/h/rs_option.h>
20
+
#include <librealsense2/h/rs_frame.h>
21
+
22
+
#include <stdio.h>
23
+
#include <assert.h>
24
+
#include <stdlib.h>
25
+
26
+
27
+
28
+
struct rs_6dof
29
+
{
30
+
struct xrt_device base;
31
+
32
+
struct xrt_pose pose;
33
+
34
+
rs2_context *ctx;
35
+
rs2_pipeline *pipe;
36
+
rs2_pipeline_profile *profile;
37
+
rs2_config *config;
38
+
};
39
+
40
+
41
+
/*!
42
+
* Helper to convert a xdev to a @ref rs_6dof.
43
+
*/
44
+
static inline struct rs_6dof *
45
+
rs_6dof(struct xrt_device *xdev)
46
+
{
47
+
return (struct rs_6dof *)xdev;
48
+
}
49
+
50
+
/*!
51
+
* Simple helper to check and print error messages.
52
+
*/
53
+
static int
54
+
check_error(struct rs_6dof *rs, rs2_error *e)
55
+
{
56
+
if (e == NULL) {
57
+
return 0;
58
+
}
59
+
60
+
fprintf(stderr, "rs_error was raised when calling %s(%s): \n",
61
+
rs2_get_failed_function(e), rs2_get_failed_args(e));
62
+
fprintf(stderr, "%s\n", rs2_get_error_message(e));
63
+
64
+
return 1;
65
+
}
66
+
67
+
/*!
68
+
* Frees all RealSense resources.
69
+
*/
70
+
static void
71
+
close_6dof(struct rs_6dof *rs)
72
+
{
73
+
if (rs->config) {
74
+
rs2_delete_config(rs->config);
75
+
rs->config = NULL;
76
+
}
77
+
78
+
if (rs->profile) {
79
+
rs2_delete_pipeline_profile(rs->profile);
80
+
rs->profile = NULL;
81
+
}
82
+
83
+
if (rs->pipe) {
84
+
rs2_pipeline_stop(rs->pipe, NULL);
85
+
rs2_delete_pipeline(rs->pipe);
86
+
rs->pipe = NULL;
87
+
}
88
+
89
+
if (rs->ctx) {
90
+
rs2_delete_context(rs->ctx);
91
+
rs->ctx = NULL;
92
+
}
93
+
}
94
+
95
+
/*!
96
+
* Create all RealSense resources needed for 6DOF tracking.
97
+
*/
98
+
static int
99
+
create_6dof(struct rs_6dof *rs)
100
+
{
101
+
assert(rs != NULL);
102
+
rs2_error *e = NULL;
103
+
104
+
rs->ctx = rs2_create_context(RS2_API_VERSION, &e);
105
+
if (check_error(rs, e) != 0) {
106
+
close_6dof(rs);
107
+
return 1;
108
+
}
109
+
110
+
rs->pipe = rs2_create_pipeline(rs->ctx, &e);
111
+
if (check_error(rs, e) != 0) {
112
+
close_6dof(rs);
113
+
return 1;
114
+
}
115
+
116
+
rs->config = rs2_create_config(&e);
117
+
if (check_error(rs, e) != 0) {
118
+
close_6dof(rs);
119
+
return 1;
120
+
}
121
+
122
+
rs2_config_enable_stream(rs->config, RS2_STREAM_POSE, 0, 0, 0,
123
+
RS2_FORMAT_6DOF, 200, &e);
124
+
if (check_error(rs, e) != 0) {
125
+
close_6dof(rs);
126
+
return 1;
127
+
}
128
+
129
+
rs->profile = rs2_pipeline_start_with_config(rs->pipe, rs->config, &e);
130
+
if (check_error(rs, e) != 0) {
131
+
close_6dof(rs);
132
+
return 1;
133
+
}
134
+
135
+
return 0;
136
+
}
137
+
138
+
/*!
139
+
* Process a frame as 6DOF data, does not assume ownership of the frame.
140
+
*/
141
+
static void
142
+
process_frame(struct rs_6dof *rs, rs2_frame *frame, struct xrt_pose *out_pose)
143
+
{
144
+
rs2_error *e = NULL;
145
+
int ret = 0;
146
+
147
+
ret = rs2_is_frame_extendable_to(frame, RS2_EXTENSION_POSE_FRAME, &e);
148
+
if (check_error(rs, e) != 0 || ret == 0) {
149
+
return;
150
+
}
151
+
152
+
rs2_pose camera_pose;
153
+
rs2_pose_frame_get_pose_data(frame, &camera_pose, &e);
154
+
if (check_error(rs, e) != 0) {
155
+
return;
156
+
}
157
+
158
+
out_pose->orientation.x = camera_pose.rotation.x;
159
+
out_pose->orientation.y = camera_pose.rotation.y;
160
+
out_pose->orientation.z = camera_pose.rotation.z;
161
+
out_pose->orientation.w = camera_pose.rotation.w;
162
+
163
+
out_pose->position.x = camera_pose.translation.x;
164
+
out_pose->position.y = camera_pose.translation.y;
165
+
out_pose->position.z = camera_pose.translation.z;
166
+
167
+
rs2_release_frame(frame);
168
+
}
169
+
170
+
static int
171
+
update(struct rs_6dof *rs, struct xrt_pose *out_pose)
172
+
{
173
+
rs2_frame *frames;
174
+
rs2_error *e = NULL;
175
+
176
+
frames =
177
+
rs2_pipeline_wait_for_frames(rs->pipe, RS2_DEFAULT_TIMEOUT, &e);
178
+
if (check_error(rs, e) != 0) {
179
+
return 1;
180
+
}
181
+
182
+
int num_frames = rs2_embedded_frames_count(frames, &e);
183
+
if (check_error(rs, e) != 0) {
184
+
return 1;
185
+
}
186
+
187
+
for (int i = 0; i < num_frames; i++) {
188
+
189
+
rs2_frame *frame = rs2_extract_frame(frames, i, &e);
190
+
if (check_error(rs, e) != 0) {
191
+
rs2_release_frame(frames);
192
+
return 1;
193
+
}
194
+
195
+
// Does not assume ownership of the frame.
196
+
process_frame(rs, frame, out_pose);
197
+
rs2_release_frame(frame);
198
+
199
+
rs2_release_frame(frames);
200
+
return 0;
201
+
}
202
+
203
+
return 0;
204
+
}
205
+
206
+
static void
207
+
rs_6dof_update_inputs(struct xrt_device *xdev, struct time_state *timekeeping)
208
+
{
209
+
// Empty
210
+
}
211
+
212
+
static void
213
+
rs_6dof_get_tracked_pose(struct xrt_device *xdev,
214
+
enum xrt_input_name name,
215
+
struct time_state *timekeeping,
216
+
int64_t *out_timestamp,
217
+
struct xrt_space_relation *out_relation)
218
+
{
219
+
struct rs_6dof *rs = rs_6dof(xdev);
220
+
221
+
if (name != XRT_INPUT_GENERIC_HEAD_POSE) {
222
+
fprintf(stderr, "unknown input name\n");
223
+
return;
224
+
}
225
+
226
+
int64_t now = time_state_get_now(timekeeping);
227
+
*out_timestamp = now;
228
+
229
+
update(rs, &rs->pose);
230
+
231
+
out_relation->pose = rs->pose;
232
+
out_relation->relation_flags = (enum xrt_space_relation_flags)(
233
+
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT |
234
+
XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT |
235
+
XRT_SPACE_RELATION_POSITION_VALID_BIT |
236
+
XRT_SPACE_RELATION_POSITION_TRACKED_BIT);
237
+
}
238
+
239
+
static void
240
+
rs_6dof_get_view_pose(struct xrt_device *xdev,
241
+
struct xrt_vec3 *eye_relation,
242
+
uint32_t view_index,
243
+
struct xrt_pose *out_pose)
244
+
{
245
+
assert(false);
246
+
}
247
+
248
+
static void
249
+
rs_6dof_destroy(struct xrt_device *xdev)
250
+
{
251
+
struct rs_6dof *rs = rs_6dof(xdev);
252
+
close_6dof(rs);
253
+
free(rs);
254
+
}
255
+
256
+
struct xrt_device *
257
+
rs_6dof_create(void)
258
+
{
259
+
struct rs_6dof *rs = U_DEVICE_ALLOCATE(
260
+
struct rs_6dof, U_DEVICE_ALLOC_TRACKING_NONE, 1, 0);
261
+
int ret = 0;
262
+
263
+
rs->base.update_inputs = rs_6dof_update_inputs;
264
+
rs->base.get_tracked_pose = rs_6dof_get_tracked_pose;
265
+
rs->base.get_view_pose = rs_6dof_get_view_pose;
266
+
rs->base.destroy = rs_6dof_destroy;
267
+
rs->base.name = XRT_DEVICE_GENERIC_HMD; // This is a lie.
268
+
rs->pose.orientation.w = 1.0f; // All other values set to zero.
269
+
270
+
// Print name.
271
+
snprintf(rs->base.str, XRT_DEVICE_NAME_LEN, "Intel RealSense 6-DOF");
272
+
273
+
// Setup input, this is a lie.
274
+
rs->base.inputs[0].name = XRT_INPUT_GENERIC_HEAD_POSE;
275
+
276
+
ret = create_6dof(rs);
277
+
if (ret != 0) {
278
+
rs_6dof_destroy(&rs->base);
279
+
return NULL;
280
+
}
281
+
282
+
return &rs->base;
283
+
}
+41
src/xrt/drivers/realsense/rs_interface.h
+41
src/xrt/drivers/realsense/rs_interface.h
···
1
+
// Copyright 2020, Collabora, Ltd.
2
+
// SPDX-License-Identifier: BSL-1.0
3
+
/*!
4
+
* @file
5
+
* @brief Interface to RealSense devices.
6
+
* @author Jakob Bornecrantz <jakob@collabora.com>
7
+
* @ingroup drv_rs
8
+
*/
9
+
10
+
#pragma once
11
+
12
+
#ifdef __cplusplus
13
+
extern "C" {
14
+
#endif
15
+
16
+
17
+
/*!
18
+
* @defgroup drv_rs North Star Driver
19
+
* @ingroup drv
20
+
*
21
+
* @brief Driver for the North Star HMD.
22
+
*/
23
+
24
+
/*!
25
+
* Create a RelaseSense 6DOF tracker device.
26
+
*
27
+
* @ingroup drv_rs
28
+
*/
29
+
struct xrt_device *
30
+
rs_6dof_create(void);
31
+
32
+
/*!
33
+
* @dir drivers/realsense
34
+
*
35
+
* @brief @ref drv_rs files.
36
+
*/
37
+
38
+
39
+
#ifdef __cplusplus
40
+
}
41
+
#endif
+5
src/xrt/targets/CMakeLists.txt
+5
src/xrt/targets/CMakeLists.txt
···
51
51
list(APPEND DRIVER_LIBRARIES ${HIDAPI_LIBRARIES})
52
52
endif()
53
53
54
+
if(BUILD_DRIVER_RS)
55
+
list(APPEND DRIVER_OBJECTS $<TARGET_OBJECTS:drv_rs>)
56
+
list(APPEND DRIVER_LIBRARIES ${realsense2_LIBRARY})
57
+
endif()
58
+
54
59
if(BUILD_DRIVER_V4L2)
55
60
list(APPEND DRIVER_OBJECTS $<TARGET_OBJECTS:drv_v4l2>)
56
61
endif()