// Copyright 2022-2023, Collabora, Ltd. // SPDX-License-Identifier: BSL-1.0 /*! * @file * @brief Builder for SimulaVR devices * @author Moshi Turner * @ingroup xrt_iface */ #include "multi_wrapper/multi.h" #include "realsense/rs_interface.h" #include "tracking/t_hand_tracking.h" #include "tracking/t_tracking.h" #include "xrt/xrt_config_drivers.h" #include "xrt/xrt_device.h" #include "xrt/xrt_prober.h" #include "util/u_builders.h" #include "util/u_config_json.h" #include "util/u_debug.h" #include "util/u_device.h" #include "util/u_sink.h" #include "util/u_system_helpers.h" #include "util/u_file.h" #include "target_builder_interface.h" #include "simula/svr_interface.h" #include "v4l2/v4l2_interface.h" #include "xrt/xrt_frameserver.h" #include "xrt/xrt_results.h" #include "xrt/xrt_tracking.h" #include DEBUG_GET_ONCE_OPTION(simula_config_path, "SIMULA_CONFIG_PATH", NULL) DEBUG_GET_ONCE_LOG_OPTION(svr_log, "SIMULA_LOG", U_LOGGING_WARN) #define SVR_TRACE(...) U_LOG_IFL_T(debug_get_log_option_svr_log(), __VA_ARGS__) #define SVR_DEBUG(...) U_LOG_IFL_D(debug_get_log_option_svr_log(), __VA_ARGS__) #define SVR_INFO(...) U_LOG_IFL_I(debug_get_log_option_svr_log(), __VA_ARGS__) #define SVR_WARN(...) U_LOG_IFL_W(debug_get_log_option_svr_log(), __VA_ARGS__) #define SVR_ERROR(...) U_LOG_IFL_E(debug_get_log_option_svr_log(), __VA_ARGS__) static const char *driver_list[] = { "simula", }; struct simula_builder { struct u_builder base; struct svr_two_displays_distortion display_distortion; }; bool process_poly_values(const cJSON *values, struct svr_display_distortion_polynomial_values *out_values) { bool good = true; good = good && u_json_get_float(u_json_get(values, "k1"), &out_values->k1); good = good && u_json_get_float(u_json_get(values, "k3"), &out_values->k3); good = good && u_json_get_float(u_json_get(values, "k5"), &out_values->k5); good = good && u_json_get_float(u_json_get(values, "k7"), &out_values->k7); good = good && u_json_get_float(u_json_get(values, "k9"), &out_values->k9); return good; } static bool process_config(const char *config_path, struct svr_two_displays_distortion *out_dist) { char *file_content = u_file_read_content_from_path(config_path, NULL); if (file_content == NULL) { U_LOG_E("The file at \"%s\" was unable to load. Either there wasn't a file there or it was empty.", config_path); return false; } cJSON *config_json = cJSON_Parse(file_content); if (config_json == NULL) { const char *error_ptr = cJSON_GetErrorPtr(); U_LOG_E("The JSON file at path \"%s\" was unable to parse", config_path); if (error_ptr != NULL) { U_LOG_E("because of an error before %s", error_ptr); } free((void *)file_content); return false; } free((void *)file_content); bool good = true; const cJSON *dd = u_json_get(config_json, "display_distortion"); if (dd == NULL) { good = false; goto end; } // struct svr_two_displays_distortion distortion = {0}; const char *eye_names[] = {"left_eye", "right_eye"}; for (int eye = 0; eye < 2; eye++) { const cJSON *this_eye = u_json_get(dd, eye_names[eye]); if (this_eye == NULL) { good = false; goto end; } // u_json does its own null checking from here on out good = good && u_json_get_float(u_json_get(this_eye, "half_fov"), &out_dist->views[eye].half_fov); good = good && u_json_get_float(u_json_get(this_eye, "display_size_mm_x"), &out_dist->views[eye].display_size_mm.x); good = good && u_json_get_float(u_json_get(this_eye, "display_size_mm_y"), &out_dist->views[eye].display_size_mm.y); good = good && process_poly_values(u_json_get(this_eye, "params_red"), &out_dist->views[eye].red); good = good && process_poly_values(u_json_get(this_eye, "params_green"), &out_dist->views[eye].green); good = good && process_poly_values(u_json_get(this_eye, "params_blue"), &out_dist->views[eye].blue); } end: cJSON_Delete(config_json); return good; } /* * * Member functions. * */ static xrt_result_t svr_estimate_system(struct xrt_builder *xb, cJSON *config, struct xrt_prober *xp, struct xrt_builder_estimate *estimate) { struct simula_builder *sb = (struct simula_builder *)xb; U_ZERO(estimate); const char *config_path = debug_get_option_simula_config_path(); if (config_path == NULL) { // No failure occurred - the user just didn't ask for Simula return XRT_SUCCESS; } bool config_valid = process_config(config_path, &sb->display_distortion); if (!config_valid) { U_LOG_E("Failed to parse SimulaVR config"); return XRT_SUCCESS; } struct xrt_prober_device **xpdevs = NULL; size_t xpdev_count = 0; xrt_result_t xret = XRT_SUCCESS; // Lock the device list xret = xrt_prober_lock_list(xp, &xpdevs, &xpdev_count); if (xret != XRT_SUCCESS) { return xret; } bool movidius = u_builder_find_prober_device(xpdevs, xpdev_count, REALSENSE_MOVIDIUS_VID, REALSENSE_MOVIDIUS_PID, XRT_BUS_TYPE_USB); bool tm2 = u_builder_find_prober_device(xpdevs, xpdev_count, REALSENSE_TM2_VID, REALSENSE_TM2_PID, XRT_BUS_TYPE_USB); if (!movidius && !tm2) { U_LOG_E("Simula enabled but couldn't find realsense device!"); return XRT_SUCCESS; } // I think that ideally we want `movidius` - in that case I think when we grab the device, it reboots to // `tm2` estimate->maybe.head = true; estimate->certain.head = true; return XRT_SUCCESS; } static xrt_result_t svr_open_system_impl(struct xrt_builder *xb, cJSON *config, struct xrt_prober *xp, struct xrt_tracking_origin *origin, struct xrt_system_devices *xsysd, struct xrt_frame_context *xfctx, struct u_builder_roles_helper *ubrh) { struct simula_builder *sb = (struct simula_builder *)xb; xrt_result_t result = XRT_SUCCESS; struct xrt_device *t265_dev = rs_create_tracked_device_internal_slam(); if (t265_dev == NULL) { SVR_ERROR("Failed to open T265 device!"); result = XRT_ERROR_DEVICE_CREATION_FAILED; goto end; } struct xrt_device *svr_dev = svr_hmd_create(&sb->display_distortion); struct xrt_pose ident = XRT_POSE_IDENTITY; struct xrt_device *head_device = multi_create_tracking_override( XRT_TRACKING_OVERRIDE_ATTACHED, svr_dev, t265_dev, XRT_INPUT_GENERIC_TRACKER_POSE, &ident); // Add to device list. xsysd->xdevs[xsysd->xdev_count++] = head_device; // Assign to role(s). ubrh->head = head_device; end: return result; } static void svr_destroy(struct xrt_builder *xb) { free(xb); } /* * * 'Exported' functions. * */ struct xrt_builder * t_builder_simula_create(void) { struct simula_builder *sb = U_TYPED_CALLOC(struct simula_builder); // xrt_builder fields. sb->base.base.estimate_system = svr_estimate_system; sb->base.base.open_system = u_builder_open_system_static_roles; sb->base.base.destroy = svr_destroy; sb->base.base.identifier = "simula"; sb->base.base.name = "SimulaVR headset"; sb->base.base.driver_identifiers = driver_list; sb->base.base.driver_identifier_count = ARRAY_SIZE(driver_list); // u_builder fields. sb->base.open_system_static_roles = svr_open_system_impl; return &sb->base.base; }