matrixfurry.com
The open source OpenXR runtime
1// Copyright 2022-2023, Collabora, Ltd.
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief Builder for SimulaVR devices
6 * @author Moshi Turner <moshiturner@protonmail.com>
7 * @ingroup xrt_iface
8 */
9
10#include "multi_wrapper/multi.h"
11#include "realsense/rs_interface.h"
12#include "tracking/t_hand_tracking.h"
13#include "tracking/t_tracking.h"
14
15#include "xrt/xrt_config_drivers.h"
16#include "xrt/xrt_device.h"
17#include "xrt/xrt_prober.h"
18
19#include "util/u_builders.h"
20#include "util/u_config_json.h"
21#include "util/u_debug.h"
22#include "util/u_device.h"
23#include "util/u_sink.h"
24#include "util/u_system_helpers.h"
25#include "util/u_file.h"
26
27#include "target_builder_interface.h"
28
29#include "simula/svr_interface.h"
30#include "v4l2/v4l2_interface.h"
31
32#include "xrt/xrt_frameserver.h"
33#include "xrt/xrt_results.h"
34#include "xrt/xrt_tracking.h"
35
36#include <assert.h>
37
38DEBUG_GET_ONCE_OPTION(simula_config_path, "SIMULA_CONFIG_PATH", NULL)
39DEBUG_GET_ONCE_LOG_OPTION(svr_log, "SIMULA_LOG", U_LOGGING_WARN)
40
41
42#define SVR_TRACE(...) U_LOG_IFL_T(debug_get_log_option_svr_log(), __VA_ARGS__)
43#define SVR_DEBUG(...) U_LOG_IFL_D(debug_get_log_option_svr_log(), __VA_ARGS__)
44#define SVR_INFO(...) U_LOG_IFL_I(debug_get_log_option_svr_log(), __VA_ARGS__)
45#define SVR_WARN(...) U_LOG_IFL_W(debug_get_log_option_svr_log(), __VA_ARGS__)
46#define SVR_ERROR(...) U_LOG_IFL_E(debug_get_log_option_svr_log(), __VA_ARGS__)
47
48static const char *driver_list[] = {
49 "simula",
50};
51
52struct simula_builder
53{
54 struct u_builder base;
55
56 struct svr_two_displays_distortion display_distortion;
57};
58
59bool
60process_poly_values(const cJSON *values, struct svr_display_distortion_polynomial_values *out_values)
61{
62 bool good = true;
63 good = good && u_json_get_float(u_json_get(values, "k1"), &out_values->k1);
64 good = good && u_json_get_float(u_json_get(values, "k3"), &out_values->k3);
65 good = good && u_json_get_float(u_json_get(values, "k5"), &out_values->k5);
66 good = good && u_json_get_float(u_json_get(values, "k7"), &out_values->k7);
67 good = good && u_json_get_float(u_json_get(values, "k9"), &out_values->k9);
68 return good;
69}
70
71static bool
72process_config(const char *config_path, struct svr_two_displays_distortion *out_dist)
73{
74 char *file_content = u_file_read_content_from_path(config_path, NULL);
75 if (file_content == NULL) {
76 U_LOG_E("The file at \"%s\" was unable to load. Either there wasn't a file there or it was empty.",
77 config_path);
78 return false;
79 }
80
81 cJSON *config_json = cJSON_Parse(file_content);
82
83
84
85 if (config_json == NULL) {
86 const char *error_ptr = cJSON_GetErrorPtr();
87 U_LOG_E("The JSON file at path \"%s\" was unable to parse", config_path);
88 if (error_ptr != NULL) {
89 U_LOG_E("because of an error before %s", error_ptr);
90 }
91 free((void *)file_content);
92 return false;
93 }
94 free((void *)file_content);
95
96 bool good = true;
97
98
99 const cJSON *dd = u_json_get(config_json, "display_distortion");
100
101 if (dd == NULL) {
102 good = false;
103 goto end;
104 }
105
106 // struct svr_two_displays_distortion distortion = {0};
107
108 const char *eye_names[] = {"left_eye", "right_eye"};
109 for (int eye = 0; eye < 2; eye++) {
110 const cJSON *this_eye = u_json_get(dd, eye_names[eye]);
111 if (this_eye == NULL) {
112 good = false;
113 goto end;
114 }
115 // u_json does its own null checking from here on out
116
117 good = good && u_json_get_float(u_json_get(this_eye, "half_fov"), &out_dist->views[eye].half_fov);
118 good = good && u_json_get_float(u_json_get(this_eye, "display_size_mm_x"),
119 &out_dist->views[eye].display_size_mm.x);
120 good = good && u_json_get_float(u_json_get(this_eye, "display_size_mm_y"),
121 &out_dist->views[eye].display_size_mm.y);
122
123 good = good && process_poly_values(u_json_get(this_eye, "params_red"), &out_dist->views[eye].red);
124 good = good && process_poly_values(u_json_get(this_eye, "params_green"), &out_dist->views[eye].green);
125 good = good && process_poly_values(u_json_get(this_eye, "params_blue"), &out_dist->views[eye].blue);
126 }
127
128end:
129
130
131
132 cJSON_Delete(config_json);
133
134 return good;
135}
136
137
138/*
139 *
140 * Member functions.
141 *
142 */
143
144static xrt_result_t
145svr_estimate_system(struct xrt_builder *xb, cJSON *config, struct xrt_prober *xp, struct xrt_builder_estimate *estimate)
146{
147 struct simula_builder *sb = (struct simula_builder *)xb;
148 U_ZERO(estimate);
149
150 const char *config_path = debug_get_option_simula_config_path();
151
152 if (config_path == NULL) {
153 // No failure occurred - the user just didn't ask for Simula
154 return XRT_SUCCESS;
155 }
156
157 bool config_valid = process_config(config_path, &sb->display_distortion);
158
159 if (!config_valid) {
160 U_LOG_E("Failed to parse SimulaVR config");
161 return XRT_SUCCESS;
162 }
163
164 struct xrt_prober_device **xpdevs = NULL;
165 size_t xpdev_count = 0;
166 xrt_result_t xret = XRT_SUCCESS;
167
168 // Lock the device list
169 xret = xrt_prober_lock_list(xp, &xpdevs, &xpdev_count);
170 if (xret != XRT_SUCCESS) {
171 return xret;
172 }
173
174 bool movidius = u_builder_find_prober_device(xpdevs, xpdev_count, REALSENSE_MOVIDIUS_VID,
175 REALSENSE_MOVIDIUS_PID, XRT_BUS_TYPE_USB);
176 bool tm2 =
177 u_builder_find_prober_device(xpdevs, xpdev_count, REALSENSE_TM2_VID, REALSENSE_TM2_PID, XRT_BUS_TYPE_USB);
178
179 if (!movidius && !tm2) {
180 U_LOG_E("Simula enabled but couldn't find realsense device!");
181 return XRT_SUCCESS;
182 }
183
184 // I think that ideally we want `movidius` - in that case I think when we grab the device, it reboots to
185 // `tm2`
186
187
188 estimate->maybe.head = true;
189 estimate->certain.head = true;
190
191
192 return XRT_SUCCESS;
193}
194
195static xrt_result_t
196svr_open_system_impl(struct xrt_builder *xb,
197 cJSON *config,
198 struct xrt_prober *xp,
199 struct xrt_tracking_origin *origin,
200 struct xrt_system_devices *xsysd,
201 struct xrt_frame_context *xfctx,
202 struct u_builder_roles_helper *ubrh)
203{
204 struct simula_builder *sb = (struct simula_builder *)xb;
205 xrt_result_t result = XRT_SUCCESS;
206
207 struct xrt_device *t265_dev = rs_create_tracked_device_internal_slam();
208 if (t265_dev == NULL) {
209 SVR_ERROR("Failed to open T265 device!");
210 result = XRT_ERROR_DEVICE_CREATION_FAILED;
211 goto end;
212 }
213
214 struct xrt_device *svr_dev = svr_hmd_create(&sb->display_distortion);
215
216 struct xrt_pose ident = XRT_POSE_IDENTITY;
217
218
219 struct xrt_device *head_device = multi_create_tracking_override(
220 XRT_TRACKING_OVERRIDE_ATTACHED, svr_dev, t265_dev, XRT_INPUT_GENERIC_TRACKER_POSE, &ident);
221
222 // Add to device list.
223 xsysd->xdevs[xsysd->xdev_count++] = head_device;
224
225 // Assign to role(s).
226 ubrh->head = head_device;
227
228end:
229 return result;
230}
231
232static void
233svr_destroy(struct xrt_builder *xb)
234{
235 free(xb);
236}
237
238
239/*
240 *
241 * 'Exported' functions.
242 *
243 */
244
245struct xrt_builder *
246t_builder_simula_create(void)
247{
248 struct simula_builder *sb = U_TYPED_CALLOC(struct simula_builder);
249
250 // xrt_builder fields.
251 sb->base.base.estimate_system = svr_estimate_system;
252 sb->base.base.open_system = u_builder_open_system_static_roles;
253 sb->base.base.destroy = svr_destroy;
254 sb->base.base.identifier = "simula";
255 sb->base.base.name = "SimulaVR headset";
256 sb->base.base.driver_identifiers = driver_list;
257 sb->base.base.driver_identifier_count = ARRAY_SIZE(driver_list);
258
259 // u_builder fields.
260 sb->base.open_system_static_roles = svr_open_system_impl;
261
262 return &sb->base.base;
263}