From 45e52dee8f9b9ceb379fdbd99c47934ac9890658 Mon Sep 17 00:00:00 2001 From: Moses Turner Date: Wed, 21 Sep 2022 07:11:02 -0500 Subject: [PATCH] d/ns: Switch NS driver to builders --- src/xrt/drivers/CMakeLists.txt | 1 - src/xrt/drivers/north_star/ns_hmd.c | 409 +++++------- src/xrt/drivers/north_star/ns_hmd.h | 79 +-- src/xrt/drivers/north_star/ns_interface.h | 9 +- src/xrt/drivers/north_star/ns_prober.c | 83 --- src/xrt/targets/common/CMakeLists.txt | 5 + .../targets/common/target_builder_interface.h | 9 + .../common/target_builder_north_star.c | 581 ++++++++++++++++++ src/xrt/targets/common/target_lists.c | 12 +- 9 files changed, 788 insertions(+), 400 deletions(-) delete mode 100644 src/xrt/drivers/north_star/ns_prober.c create mode 100644 src/xrt/targets/common/target_builder_north_star.c diff --git a/src/xrt/drivers/CMakeLists.txt b/src/xrt/drivers/CMakeLists.txt index 3f2b0377f..b044c92c1 100644 --- a/src/xrt/drivers/CMakeLists.txt +++ b/src/xrt/drivers/CMakeLists.txt @@ -105,7 +105,6 @@ if(XRT_BUILD_DRIVER_NS) north_star/ns_hmd.h north_star/ns_hmd.c north_star/ns_interface.h - north_star/ns_prober.c ) target_link_libraries(drv_ns PRIVATE xrt-interfaces aux_math xrt-external-cjson) list(APPEND ENABLED_HEADSET_DRIVERS ns) diff --git a/src/xrt/drivers/north_star/ns_hmd.c b/src/xrt/drivers/north_star/ns_hmd.c index 3ed896d9a..b284704de 100644 --- a/src/xrt/drivers/north_star/ns_hmd.c +++ b/src/xrt/drivers/north_star/ns_hmd.c @@ -32,14 +32,22 @@ DEBUG_GET_ONCE_LOG_OPTION(ns_log, "NS_LOG", U_LOGGING_INFO) -#define printf_pose(pose) \ - printf("%f %f %f %f %f %f %f\n", pose.position.x, pose.position.y, pose.position.z, pose.orientation.x, \ - pose.orientation.y, pose.orientation.z, pose.orientation.w); +/* + * + * Printing functions. + * + */ + +#define NS_TRACE(d, ...) U_LOG_XDEV_IFL_T(&d->base, d->log_level, __VA_ARGS__) +#define NS_DEBUG(d, ...) U_LOG_XDEV_IFL_D(&d->base, d->log_level, __VA_ARGS__) +#define NS_INFO(d, ...) U_LOG_XDEV_IFL_I(&d->base, d->log_level, __VA_ARGS__) +#define NS_WARN(d, ...) U_LOG_XDEV_IFL_W(&d->base, d->log_level, __VA_ARGS__) +#define NS_ERROR(d, ...) U_LOG_XDEV_IFL_E(&d->base, d->log_level, __VA_ARGS__) static float try_get_ipd(struct ns_hmd *ns, const struct cJSON *json) -{ // +{ const char *things[] = {"baseline", "ipd", "IPD"}; bool done = false; float out; @@ -50,7 +58,8 @@ try_get_ipd(struct ns_hmd *ns, const struct cJSON *json) } if (!done) { NS_INFO(ns, - "No key `baseline (or ipd, or IPD)` in your config file. Guessing the IPD is 64 millimeters"); + "No key `baseline` (or `ipd`, or `IPD`) in your config file. " + "Guessing the IPD is 64 millimeters"); out = 64.0f; } if (out > 250.0f) { @@ -68,7 +77,7 @@ try_get_ipd(struct ns_hmd *ns, const struct cJSON *json) } static void -try_get_fov(struct ns_hmd *ns, const struct cJSON *json, struct xrt_fov *left_fov, struct xrt_fov *right_fov) +try_get_fov(struct ns_hmd *ns, const struct cJSON *json, struct xrt_fov *out_left_fov, struct xrt_fov *out_right_fov) { const char *things[] = {"fov", "FOV"}; float out_float; @@ -105,108 +114,59 @@ good: assert(fabsf(out_fov.angle_down) < M_PI_2); assert(fabsf(out_fov.angle_left) < M_PI_2); assert(fabsf(out_fov.angle_right) < M_PI_2); - memcpy(left_fov, &out_fov, sizeof(struct xrt_fov)); - memcpy(right_fov, &out_fov, sizeof(struct xrt_fov)); + *out_left_fov = out_fov; + *out_right_fov = out_fov; } -/* - * - * "2D Polynomial" distortion; original implementation by Johnathon Zelstadt - * Sometimes known as "v2", filename is often NorthStarCalibration.json - * - */ - -static bool -ns_p2d_mesh_calc(struct xrt_device *xdev, int view, float u, float v, struct xrt_uv_triplet *result) -{ - struct ns_hmd *ns = ns_hmd(xdev); - return u_compute_distortion_ns_p2d(&ns->dist_p2d, view, u, v, result); -} bool -ns_p2d_parse(struct ns_hmd *ns) +ns_p2d_parse(struct ns_hmd *ns, const cJSON *json) { - - struct xrt_pose temp_eyes_center_to_eye[2]; - - // convenience names - const struct cJSON *config_json = ns->config_json; + struct u_ns_p2d_values *values = &ns->config.dist_p2d; // Note that x and y are flipped. We have to flip 'em at some point - the polynomial calibrator has a strange // definition of x and y. "opencv treats column major over row major (as in, Y,X for image look up)" -Dr. Damo - if (u_json_get_float_array(u_json_get(config_json, "left_uv_to_rect_x"), ns->dist_p2d.y_coefficients_right, - 16) != 16) + if (u_json_get_float_array(u_json_get(json, "left_uv_to_rect_x"), values->y_coefficients_right, 16) != 16) goto cleanup_p2d; - if (u_json_get_float_array(u_json_get(config_json, "left_uv_to_rect_y"), ns->dist_p2d.x_coefficients_right, - 16) != 16) + if (u_json_get_float_array(u_json_get(json, "left_uv_to_rect_y"), values->x_coefficients_right, 16) != 16) goto cleanup_p2d; - if (u_json_get_float_array(u_json_get(config_json, "right_uv_to_rect_x"), ns->dist_p2d.y_coefficients_left, - 16) != 16) + if (u_json_get_float_array(u_json_get(json, "right_uv_to_rect_x"), values->y_coefficients_left, 16) != 16) goto cleanup_p2d; - if (u_json_get_float_array(u_json_get(config_json, "right_uv_to_rect_y"), ns->dist_p2d.x_coefficients_left, - 16) != 16) + if (u_json_get_float_array(u_json_get(json, "right_uv_to_rect_y"), values->x_coefficients_left, 16) != 16) goto cleanup_p2d; // at this point, locked into using this distortion method - we can touch anything and not worry about side // effects - float baseline = try_get_ipd(ns, config_json); + ns->config.distortion_type = NS_DISTORTION_TYPE_POLYNOMIAL_2D; - math_pose_identity(&temp_eyes_center_to_eye[0]); - math_pose_identity(&temp_eyes_center_to_eye[1]); - temp_eyes_center_to_eye[0].position.x = -baseline / 2; - temp_eyes_center_to_eye[1].position.x = baseline / 2; + float baseline = try_get_ipd(ns, json); - try_get_fov(ns, config_json, &ns->dist_p2d.fov[0], &ns->dist_p2d.fov[1]); + math_pose_identity(&ns->config.head_pose_to_eye[0]); + math_pose_identity(&ns->config.head_pose_to_eye[1]); + ns->config.head_pose_to_eye[0].position.x = -baseline / 2; + ns->config.head_pose_to_eye[1].position.x = baseline / 2; - memcpy(&ns->base.hmd->distortion.fov[0], &ns->dist_p2d.fov[0], sizeof(struct xrt_fov)); - memcpy(&ns->base.hmd->distortion.fov[1], &ns->dist_p2d.fov[1], sizeof(struct xrt_fov)); + try_get_fov(ns, json, &values->fov[0], &values->fov[1]); - ns->base.compute_distortion = &ns_p2d_mesh_calc; - memcpy(&ns->head_pose_to_eye, &temp_eyes_center_to_eye, sizeof(struct xrt_pose) * 2); + ns->config.fov[0] = values->fov[0]; + ns->config.fov[1] = values->fov[1]; return true; cleanup_p2d: - memset(&ns->dist_p2d, 0, sizeof(struct u_ns_p2d_values)); return false; } -/* - * - * "Original 3D" undistortion, by Leap Motion - * Sometimes known as "v1", config file name is often "Calibration.json" - * - */ - -static bool -ns_3d_mesh_calc(struct xrt_device *xdev, int view, float u, float v, struct xrt_uv_triplet *result) -{ - struct ns_hmd *ns = ns_hmd(xdev); - struct ns_3d_data *data = &ns->dist_3d; - struct xrt_vec2 uv = {u, v}; - struct xrt_vec2 warped_uv = {0.0f, 0.0f}; - - ns_3d_display_uv_to_render_uv(uv, &warped_uv, &data->eyes[view]); - - result->r.x = warped_uv.x; - result->r.y = warped_uv.y; - result->g.x = warped_uv.x; - result->g.y = warped_uv.y; - result->b.x = warped_uv.x; - result->b.y = warped_uv.y; - return true; -} - static void -ns_3d_fov_calculate(struct xrt_fov *fov, struct xrt_quat projection) +ns_3d_fov_calculate(struct xrt_quat projection, struct xrt_fov *out_fov) { // Million thanks to Nico Zobernig for figuring this out - fov->angle_left = atanf(projection.x); - fov->angle_right = atanf(projection.y); - fov->angle_up = atanf(projection.z); - fov->angle_down = atanf(projection.w); + out_fov->angle_left = atanf(projection.x); + out_fov->angle_right = atanf(projection.y); + out_fov->angle_up = atanf(projection.z); + out_fov->angle_down = atanf(projection.w); } /* @@ -215,20 +175,10 @@ ns_3d_fov_calculate(struct xrt_fov *fov, struct xrt_quat projection) * */ -static bool -ns_3d_leap_parse(struct ns_3d_leap *leap, const struct cJSON *leap_data) -{ - u_json_get_string_into_array(u_json_get(leap_data, "name"), leap->name, 64); - u_json_get_string_into_array(u_json_get(leap_data, "serial"), leap->serial, 64); - if (!u_json_get_vec3(u_json_get(u_json_get(leap_data, "localPose"), "position"), &leap->pose.position)) - return false; - if (!u_json_get_quat(u_json_get(u_json_get(leap_data, "localPose"), "rotation"), &leap->pose.orientation)) - return false; - return true; -} + static bool -ns_3d_eye_parse(struct ns_3d_eye *eye, const struct cJSON *eye_data) +ns_3d_eye_parse(struct ns_hmd *ns, struct ns_3d_eye *eye, const struct cJSON *eye_data) { if (!u_json_get_float(u_json_get(eye_data, "ellipseMinorAxis"), &eye->ellipse_minor_axis)) return false; @@ -259,86 +209,66 @@ ns_3d_eye_parse(struct ns_3d_eye *eye, const struct cJSON *eye_data) } bool -ns_3d_parse(struct ns_hmd *ns) +ns_3d_parse(struct ns_hmd *ns, const cJSON *json) { - struct ns_3d_data *our_ns_3d_data = &ns->dist_3d; + struct ns_3d_values *values = &ns->config.dist_3d; - if (!ns_3d_eye_parse(&our_ns_3d_data->eyes[0], u_json_get(ns->config_json, "leftEye"))) + + if (!ns_3d_eye_parse(ns, &values->eyes[0], u_json_get(json, "leftEye"))) goto cleanup_l3d; - if (!ns_3d_eye_parse(&our_ns_3d_data->eyes[1], u_json_get(ns->config_json, "rightEye"))) - goto cleanup_l3d; - if (!ns_3d_leap_parse(&our_ns_3d_data->leap, u_json_get(ns->config_json, "leapTracker"))) + if (!ns_3d_eye_parse(ns, &values->eyes[1], u_json_get(json, "rightEye"))) goto cleanup_l3d; // Locked in, okay to touch anything inside ns struct - ns_3d_fov_calculate(&ns->base.hmd->distortion.fov[0], our_ns_3d_data->eyes[0].camera_projection); - ns_3d_fov_calculate(&ns->base.hmd->distortion.fov[1], our_ns_3d_data->eyes[1].camera_projection); + ns->config.distortion_type = NS_DISTORTION_TYPE_GEOMETRIC_3D; - ns->head_pose_to_eye[0] = our_ns_3d_data->eyes[0].eye_pose; // Left eye. - ns->head_pose_to_eye[1] = our_ns_3d_data->eyes[1].eye_pose; // Right eye. + ns_3d_fov_calculate(values->eyes[0].camera_projection, &ns->config.fov[0]); + ns_3d_fov_calculate(values->eyes[1].camera_projection, &ns->config.fov[1]); - our_ns_3d_data->eyes[0].optical_system = ns_3d_create_optical_system(&our_ns_3d_data->eyes[0]); - our_ns_3d_data->eyes[1].optical_system = ns_3d_create_optical_system(&our_ns_3d_data->eyes[1]); + ns->config.head_pose_to_eye[0] = values->eyes[0].eye_pose; // Left eye. + ns->config.head_pose_to_eye[1] = values->eyes[1].eye_pose; // Right eye. - ns->base.compute_distortion = &ns_3d_mesh_calc; + values->eyes[0].optical_system = ns_3d_create_optical_system(&values->eyes[0]); + values->eyes[1].optical_system = ns_3d_create_optical_system(&values->eyes[1]); return true; cleanup_l3d: - memset(&ns->dist_3d, 0, sizeof(struct ns_3d_data)); + ns_3d_free_optical_system(&values->eyes[0].optical_system); + ns_3d_free_optical_system(&values->eyes[1].optical_system); return false; } /* * - * Moses Turner's distortion correction + * Moses Turner's meshgrid-based distortion correction * */ bool -ns_meshgrid_mesh_calc(struct xrt_device *xdev, int view, float u, float v, struct xrt_uv_triplet *result) +ns_mt_parse(struct ns_hmd *ns, const cJSON *json) { - struct ns_hmd *ns = ns_hmd(xdev); - return u_compute_distortion_ns_meshgrid(&ns->dist_meshgrid, view, u, v, result); -} + struct u_ns_meshgrid_values *values = &ns->config.dist_meshgrid; -void -ns_meshgrid_free_values(struct ns_hmd *ns) -{ - free(ns->dist_meshgrid.ipds); - free(ns->dist_meshgrid.grid[0]); - free(ns->dist_meshgrid.grid[1]); -} - -bool -ns_meshgrid_parse(struct ns_hmd *ns) -{ - - struct u_ns_meshgrid_values *values = &ns->dist_meshgrid; - const struct cJSON *config_json = ns->config_json; - - if (strcmp(cJSON_GetStringValue(u_json_get(config_json, "type")), "Moses Turner's distortion correction") != - 0) { + if (strcmp(cJSON_GetStringValue(u_json_get(json, "type")), "Moses Turner's distortion correction") != 0) { goto cleanup_mt; } int version = 0; - u_json_get_int(u_json_get(config_json, "version"), &version); + u_json_get_int(u_json_get(json, "version"), &version); if (version != 2) { goto cleanup_mt; } - u_json_get_int(u_json_get(config_json, "num_grid_points_x"), &values->num_grid_points_u); - u_json_get_int(u_json_get(config_json, "num_grid_points_y"), &values->num_grid_points_v); + u_json_get_int(u_json_get(json, "num_grid_points_x"), &values->num_grid_points_u); + u_json_get_int(u_json_get(json, "num_grid_points_y"), &values->num_grid_points_v); - values->grid[0] = - realloc(values->grid[0], sizeof(struct xrt_vec2) * values->num_grid_points_u * values->num_grid_points_v); - values->grid[1] = - realloc(values->grid[1], sizeof(struct xrt_vec2) * values->num_grid_points_u * values->num_grid_points_v); + values->grid[0] = U_TYPED_ARRAY_CALLOC(struct xrt_vec2, values->num_grid_points_u * values->num_grid_points_v); + values->grid[1] = U_TYPED_ARRAY_CALLOC(struct xrt_vec2, values->num_grid_points_u * values->num_grid_points_v); - values->ipd = try_get_ipd(ns, ns->config_json); + values->ipd = try_get_ipd(ns, json); - const cJSON *current_element = config_json; + const cJSON *current_element = json; for (int view = 0; view <= 1; view++) { @@ -365,44 +295,52 @@ ns_meshgrid_parse(struct ns_hmd *ns) } } } + // locked in + ns->config.distortion_type = NS_DISTORTION_TYPE_MOSES_MESHGRID; float baseline = values->ipd; - try_get_fov(ns, config_json, &values->fov[0], &values->fov[1]); + try_get_fov(ns, json, &values->fov[0], &values->fov[1]); - ns->base.hmd->distortion.fov[0] = values->fov[0]; - ns->base.hmd->distortion.fov[1] = values->fov[1]; + ns->config.fov[0] = values->fov[0]; + ns->config.fov[1] = values->fov[1]; - ns->head_pose_to_eye[0].orientation.x = 0.0f; - ns->head_pose_to_eye[0].orientation.y = 0.0f; - ns->head_pose_to_eye[0].orientation.z = 0.0f; - ns->head_pose_to_eye[0].orientation.w = 1.0f; - ns->head_pose_to_eye[0].position.x = -baseline / 2; - ns->head_pose_to_eye[0].position.y = 0.0f; - ns->head_pose_to_eye[0].position.z = 0.0f; - - - - ns->head_pose_to_eye[1].orientation.x = 0.0f; - ns->head_pose_to_eye[1].orientation.y = 0.0f; - ns->head_pose_to_eye[1].orientation.z = 0.0f; - ns->head_pose_to_eye[1].orientation.w = 1.0f; - ns->head_pose_to_eye[1].position.x = baseline / 2; - ns->head_pose_to_eye[1].position.y = 0.0f; - ns->head_pose_to_eye[1].position.z = 0.0f; - - ns->base.compute_distortion = &ns_meshgrid_mesh_calc; - - ns->free_distortion_values = ns_meshgrid_free_values; + math_pose_identity(&ns->config.head_pose_to_eye[0]); + math_pose_identity(&ns->config.head_pose_to_eye[1]); + ns->config.head_pose_to_eye[0].position.x = -baseline / 2; + ns->config.head_pose_to_eye[1].position.x = baseline / 2; return true; cleanup_mt: - memset(&ns->dist_meshgrid, 0, sizeof(struct u_ns_meshgrid_values)); + free(values->grid[0]); + free(values->grid[1]); return false; } + + +static bool +ns_optical_config_parse(struct ns_hmd *ns) +{ + if (ns_3d_parse(ns, ns->config_json)) { + NS_INFO(ns, "Using Gemetric 3D display distortion correction!"); + return true; + } + if (ns_p2d_parse(ns, ns->config_json)) { + NS_INFO(ns, "Using Polynomial 2D display distortion correction!"); + return true; + } + if (ns_mt_parse(ns, ns->config_json)) { + NS_INFO(ns, "Using Moses's meshgrid-based display distortion correction!"); + return true; + } + U_LOG_E("Couldn't find a valid display distortion correction!"); + return false; +} + + /* * * Common functions @@ -413,12 +351,17 @@ static void ns_hmd_destroy(struct xrt_device *xdev) { struct ns_hmd *ns = ns_hmd(xdev); + NS_DEBUG(ns, "Called!"); // Remove the variable tracking. u_var_remove_root(ns); - if (ns->free_distortion_values != NULL) { - ns->free_distortion_values(ns); + if (ns->config.distortion_type == NS_DISTORTION_TYPE_GEOMETRIC_3D) { + ns_3d_free_optical_system(&ns->config.dist_3d.eyes[0].optical_system); + ns_3d_free_optical_system(&ns->config.dist_3d.eyes[1].optical_system); + } else if (ns->config.distortion_type == NS_DISTORTION_TYPE_MOSES_MESHGRID) { + free(ns->config.dist_meshgrid.grid[0]); + free(ns->config.dist_meshgrid.grid[1]); } u_device_free(&ns->base); @@ -426,7 +369,10 @@ ns_hmd_destroy(struct xrt_device *xdev) static void ns_hmd_update_inputs(struct xrt_device *xdev) -{} +{ + struct ns_hmd *ns = ns_hmd(xdev); + NS_DEBUG(ns, "Called!"); +} static void ns_hmd_get_tracked_pose(struct xrt_device *xdev, @@ -435,6 +381,7 @@ ns_hmd_get_tracked_pose(struct xrt_device *xdev, struct xrt_space_relation *out_relation) { struct ns_hmd *ns = ns_hmd(xdev); + NS_DEBUG(ns, "Called!"); if (name != XRT_INPUT_GENERIC_HEAD_POSE) { NS_ERROR(ns, "unknown input name"); @@ -453,90 +400,53 @@ ns_hmd_get_view_poses(struct xrt_device *xdev, struct xrt_fov *out_fovs, struct xrt_pose *out_poses) { + struct ns_hmd *ns = ns_hmd(xdev); + NS_DEBUG(ns, "Called!"); + // Use this to take care of most stuff, then fix up below. u_device_get_view_poses(xdev, default_eye_relation, at_timestamp_ns, view_count, out_head_relation, out_fovs, out_poses); // Fix fix. - struct ns_hmd *ns = ns_hmd(xdev); - for (uint32_t i = 0; i < view_count && i < ARRAY_SIZE(ns->head_pose_to_eye); i++) { - out_poses[i] = ns->head_pose_to_eye[i]; + for (uint32_t i = 0; i < view_count && i < ARRAY_SIZE(ns->config.head_pose_to_eye); i++) { + out_poses[i] = ns->config.head_pose_to_eye[i]; } } -static bool -ns_config_load(struct ns_hmd *ns, const char *config_path) +bool +ns_mesh_calc(struct xrt_device *xdev, int view, float u, float v, struct xrt_uv_triplet *result) { - // Get the path to the JSON file - bool json_allocated = false; - if (config_path == NULL || strcmp(config_path, "/") == 0) { - NS_INFO(ns, - "Configuration path \"%s\" does not lead to a " - "configuration JSON file. Set the NS_CONFIG_PATH env " - "variable to your JSON.", - config_path); + struct ns_hmd *ns = ns_hmd(xdev); + NS_DEBUG(ns, "Called!"); + // struct xrt_vec2 warped_uv; + switch (ns->config.distortion_type) { + case NS_DISTORTION_TYPE_GEOMETRIC_3D: { + struct xrt_vec2 uv = {u, v}; + struct xrt_vec2 warped_uv = {0.0f, 0.0f}; + + ns_3d_display_uv_to_render_uv(uv, &warped_uv, &ns->config.dist_3d.eyes[view]); + + result->r.x = warped_uv.x; + result->r.y = warped_uv.y; + result->g.x = warped_uv.x; + result->g.y = warped_uv.y; + result->b.x = warped_uv.x; + result->b.y = warped_uv.y; + return true; + } + case NS_DISTORTION_TYPE_POLYNOMIAL_2D: { + return u_compute_distortion_ns_p2d(&ns->config.dist_p2d, view, u, v, result); + } + case NS_DISTORTION_TYPE_MOSES_MESHGRID: { + return u_compute_distortion_ns_meshgrid(&ns->config.dist_meshgrid, view, u, v, result); + } + default: { + assert(false); return false; } - // Open the JSON file and put its contents into a string - FILE *config_file = fopen(config_path, "r"); - if (config_file == NULL) { - NS_INFO(ns, "The configuration file at path \"%s\" was unable to load", config_path); - goto parse_error; } - - fseek(config_file, 0, SEEK_END); // Go to end of file - long file_size = ftell(config_file); // See offset we're at. This should be the file size in bytes. - rewind(config_file); // Go back to the beginning of the file - - if (file_size == 0) { - NS_INFO(ns, "Empty config file!"); - goto parse_error; - } else if (file_size > 3 * pow(1024, 2)) { // 3 MiB - NS_INFO(ns, "Huge config file! (%f MiB!!) Something's wrong here.", ((float)file_size) / pow(1024, 2)); - goto parse_error; - } - - char *json = calloc(file_size + 1, 1); - json_allocated = true; - - size_t ret = fread(json, 1, file_size, config_file); - if ((long)ret != file_size) { - NS_ERROR(ns, "Failed to read configuration file at path \"%s\"", config_path); - goto parse_error; - } - fclose(config_file); - config_file = NULL; - json[file_size] = '\0'; - - ns->config_json = cJSON_Parse(json); - if (ns->config_json == NULL) { - const char *error_ptr = cJSON_GetErrorPtr(); - NS_INFO(ns, "The JSON file at path \"%s\" was unable to parse", config_path); - if (error_ptr != NULL) { - NS_INFO(ns, "because of an error before %s", error_ptr); - } - goto parse_error; - } - - // this function is not supposed to return true if ns->config_json is NULL - assert(ns->config_json != NULL); - free(json); - - return true; - -parse_error: - if (config_file != NULL) { - fclose(config_file); - config_file = NULL; - } - if (json_allocated) { - free(json); - } - NS_INFO(ns, "Are you sure you're using the right configuration file?"); - return false; } - /* * * Create function. @@ -544,36 +454,23 @@ parse_error: */ struct xrt_device * -ns_hmd_create(const char *config_path) +ns_hmd_create(const cJSON *config_json) { enum u_device_alloc_flags flags = (enum u_device_alloc_flags)(U_DEVICE_ALLOC_HMD | U_DEVICE_ALLOC_TRACKING_NONE); struct ns_hmd *ns = U_DEVICE_ALLOCATE(struct ns_hmd, flags, 1, 0); + + ns->config_json = config_json; + ns_optical_config_parse(ns); + ns->log_level = debug_get_log_option_ns_log(); + NS_DEBUG(ns, "Called!"); - if (!ns_config_load(ns, config_path)) - goto cleanup; // don't need to print any error, ns_config_load did that for us + ns->base.hmd->distortion.fov[0] = ns->config.fov[0]; + ns->base.hmd->distortion.fov[1] = ns->config.fov[1]; - int number_wrap = 3; // number of elements in below array of function pointers. Const to stop compiler warnings. - bool (*wrap_func_ptr[3])(struct ns_hmd *) = {ns_3d_parse, ns_p2d_parse, ns_meshgrid_parse}; - // C syntax is weird here. This is an array of pointers to functions with arguments (struct ns_system * system) - // that all return a boolean value. The array should be roughly in descending order of how likely we think the - // user means to use each method For now `meshgrid` is last because Moses is the only one that uses it - - bool found_config_wrap = false; - for (int i = 0; i < number_wrap; i++) { - if (wrap_func_ptr[i](ns)) { // wrap_func_ptr[i](ns) is a function call! - U_LOG_I("North Star: Using config wrap %i", i); - found_config_wrap = true; - break; - } - } // This will segfault at function ?? if you use GDB and the length is wrong. - - if (!found_config_wrap) { - NS_INFO(ns, "North Star: Config file seems to be invalid."); - goto cleanup; - } + ns->base.compute_distortion = ns_mesh_calc; ns->base.update_inputs = ns_hmd_update_inputs; ns->base.get_tracked_pose = ns_hmd_get_tracked_pose; ns->base.get_view_poses = ns_hmd_get_view_poses; @@ -645,8 +542,4 @@ ns_hmd_create(const char *config_path) return &ns->base; - -cleanup: - ns_hmd_destroy(&ns->base); - return NULL; } diff --git a/src/xrt/drivers/north_star/ns_hmd.h b/src/xrt/drivers/north_star/ns_hmd.h index cedfe94e0..89592b68e 100644 --- a/src/xrt/drivers/north_star/ns_hmd.h +++ b/src/xrt/drivers/north_star/ns_hmd.h @@ -27,18 +27,6 @@ extern "C" { #endif -/* - * - * Printing functions. - * - */ - -#define NS_TRACE(d, ...) U_LOG_XDEV_IFL_T(&d->base, d->log_level, __VA_ARGS__) -#define NS_DEBUG(d, ...) U_LOG_XDEV_IFL_D(&d->base, d->log_level, __VA_ARGS__) -#define NS_INFO(d, ...) U_LOG_XDEV_IFL_I(&d->base, d->log_level, __VA_ARGS__) -#define NS_WARN(d, ...) U_LOG_XDEV_IFL_W(&d->base, d->log_level, __VA_ARGS__) -#define NS_ERROR(d, ...) U_LOG_XDEV_IFL_E(&d->base, d->log_level, __VA_ARGS__) - /* * * 3D distortion structs @@ -53,19 +41,6 @@ extern "C" { */ struct ns_3d_optical_system; -/*! - * Configuration information about the LMC or Rigel sensor according to the - * configuration file. - * - * @ingroup drv_ns - */ -struct ns_3d_leap -{ - char name[64]; - char serial[64]; - struct xrt_pose pose; -}; - /*! * Distortion information about an eye parsed from the configuration file. * @@ -81,6 +56,7 @@ struct ns_3d_eye struct xrt_pose eye_pose; + // This is more of a vec4 than a quat struct xrt_quat camera_projection; struct xrt_matrix_4x4 sphere_to_world_space; @@ -89,12 +65,37 @@ struct ns_3d_eye struct ns_optical_system *optical_system; }; -struct ns_3d_data +struct ns_3d_values { struct ns_3d_eye eyes[2]; - struct ns_3d_leap leap; }; +enum ns_distortion_type +{ + NS_DISTORTION_TYPE_INVALID, + NS_DISTORTION_TYPE_GEOMETRIC_3D, + NS_DISTORTION_TYPE_POLYNOMIAL_2D, + NS_DISTORTION_TYPE_MOSES_MESHGRID, +}; + +// The config json data gets dumped in here. +// In general, `target_builder_north_star.c` sets up tracking, and `ns_hmd.c` sets up distortion/optics. +struct ns_optics_config +{ + struct xrt_pose head_pose_to_eye[2]; // left, right + struct xrt_fov fov[2]; // left,right + + + + enum ns_distortion_type distortion_type; + union { + struct ns_3d_values dist_3d; + struct u_ns_p2d_values dist_p2d; + struct u_ns_meshgrid_values dist_meshgrid; + }; +}; + + /*! * Information about the whole North Star headset. * @@ -106,16 +107,8 @@ struct ns_hmd { struct xrt_device base; struct xrt_space_relation no_tracker_relation; - const char *config_path; - struct cJSON *config_json; - struct xrt_pose head_pose_to_eye[2]; // left, right - - void (*free_distortion_values)(struct ns_hmd *hmd); - union { - struct ns_3d_data dist_3d; - struct u_ns_p2d_values dist_p2d; - struct u_ns_meshgrid_values dist_meshgrid; - }; + const cJSON *config_json; + struct ns_optics_config config; enum u_logging_level log_level; }; @@ -138,15 +131,6 @@ ns_hmd(struct xrt_device *xdev) } -/*! - * Create a North Star hmd. - * - * @ingroup drv_ns - */ -struct xrt_device * -ns_hmd_create(const char *config_path); - - /*! * Convert the display UV to the render UV using the distortion mesh. * @@ -159,6 +143,9 @@ ns_3d_display_uv_to_render_uv(struct xrt_vec2 in, struct xrt_vec2 *out, struct n struct ns_optical_system * ns_3d_create_optical_system(struct ns_3d_eye *eye); +void +ns_3d_free_optical_system(struct ns_optical_system **system); + #ifdef __cplusplus } diff --git a/src/xrt/drivers/north_star/ns_interface.h b/src/xrt/drivers/north_star/ns_interface.h index 4ccfb255d..995ad9112 100644 --- a/src/xrt/drivers/north_star/ns_interface.h +++ b/src/xrt/drivers/north_star/ns_interface.h @@ -9,6 +9,8 @@ */ #pragma once +#include "util/u_json.h" +#include "xrt/xrt_device.h" #ifdef __cplusplus extern "C" { @@ -23,12 +25,13 @@ extern "C" { */ /*! - * Create a probe for NS devices. + * Creates a North Star HMD. * * @ingroup drv_ns */ -struct xrt_auto_prober * -ns_create_auto_prober(void); + +struct xrt_device * +ns_hmd_create(const cJSON *config_json); /*! * @dir drivers/north_star diff --git a/src/xrt/drivers/north_star/ns_prober.c b/src/xrt/drivers/north_star/ns_prober.c deleted file mode 100644 index bcd8383a1..000000000 --- a/src/xrt/drivers/north_star/ns_prober.c +++ /dev/null @@ -1,83 +0,0 @@ -// Copyright 2019-2020, Collabora, Ltd. -// Copyright 2020, Nova King. -// SPDX-License-Identifier: BSL-1.0 -/*! - * @file - * @brief North Star prober code. - * @author Nova King - * @author Jakob Bornecrantz - * @ingroup drv_ns - */ - -#include -#include - -#include "xrt/xrt_prober.h" - -#include "util/u_misc.h" -#include "util/u_debug.h" - -#include "ns_interface.h" -#include "ns_hmd.h" - - -DEBUG_GET_ONCE_OPTION(ns_config_path, "NS_CONFIG_PATH", NULL) - -/*! - * @implements xrt_auto_prober - */ -struct ns_prober -{ - struct xrt_auto_prober base; - const char *config_path; -}; - -//! @private @memberof ns_prober -static inline struct ns_prober * -ns_prober(struct xrt_auto_prober *p) -{ - return (struct ns_prober *)p; -} - -//! @public @memberof ns_prober -static void -ns_prober_destroy(struct xrt_auto_prober *p) -{ - struct ns_prober *nsp = ns_prober(p); - - free(nsp); -} - -//! @public @memberof ns_prober -static int -ns_prober_autoprobe(struct xrt_auto_prober *xap, - cJSON *attached_data, - bool no_hmds, - struct xrt_prober *xp, - struct xrt_device **out_xdevs) -{ - struct ns_prober *nsp = ns_prober(xap); - - if (no_hmds) { - return 0; - } - - if (nsp->config_path == NULL) { - return 0; - } - - out_xdevs[0] = ns_hmd_create(nsp->config_path); - return 1; -} - -struct xrt_auto_prober * -ns_create_auto_prober() -{ - struct ns_prober *nsp = U_TYPED_CALLOC(struct ns_prober); - nsp->base.name = "northstar"; - nsp->base.destroy = ns_prober_destroy; - nsp->base.lelo_dallas_autoprobe = ns_prober_autoprobe; - nsp->config_path = debug_get_option_ns_config_path(); - - return &nsp->base; -} diff --git a/src/xrt/targets/common/CMakeLists.txt b/src/xrt/targets/common/CMakeLists.txt index 8de4a893b..8f8efddb8 100644 --- a/src/xrt/targets/common/CMakeLists.txt +++ b/src/xrt/targets/common/CMakeLists.txt @@ -37,6 +37,11 @@ if(XRT_BUILD_DRIVER_SIMULAVR) target_sources(target_lists PRIVATE target_builder_simulavr.c) endif() +if(XRT_BUILD_DRIVER_NS) + target_sources(target_lists PRIVATE target_builder_north_star.c) + target_link_libraries(target_lists PRIVATE drv_ns) +endif() + ### # Drivers # diff --git a/src/xrt/targets/common/target_builder_interface.h b/src/xrt/targets/common/target_builder_interface.h index 58273b5a1..d4b62bb06 100644 --- a/src/xrt/targets/common/target_builder_interface.h +++ b/src/xrt/targets/common/target_builder_interface.h @@ -32,6 +32,10 @@ #define T_BUILDER_SIMULAVR #endif +#if defined(XRT_BUILD_DRIVER_NS) +#define T_BUILDER_NS +#endif + // Always enabled. #define T_BUILDER_LEGACY @@ -81,3 +85,8 @@ t_builder_lighthouse_create(void); struct xrt_builder * t_builder_simula_create(void); #endif + +#ifdef T_BUILDER_NS +struct xrt_builder * +t_builder_north_star_create(void); +#endif diff --git a/src/xrt/targets/common/target_builder_north_star.c b/src/xrt/targets/common/target_builder_north_star.c new file mode 100644 index 000000000..3d5cd888f --- /dev/null +++ b/src/xrt/targets/common/target_builder_north_star.c @@ -0,0 +1,581 @@ +// Copyright 2022, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief System builder for North Star headsets + * @author Nova King + * @author Jakob Bornecrantz + * @author Moses Turner + * @ingroup xrt_iface + */ + +#include "math/m_api.h" +#include "math/m_space.h" +#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 "util/u_pretty_print.h" + +#include "target_builder_interface.h" + +#include "north_star/ns_interface.h" + +#ifdef XRT_BUILD_DRIVER_ULV2 +#include "ultraleap_v2/ulv2_interface.h" +#endif + +#ifdef XRT_BUILD_DRIVER_REALSENSE +#include "realsense/rs_interface.h" +#endif + +#ifdef XRT_BUILD_DRIVER_DEPTHAI +#include "depthai/depthai_interface.h" +#endif + +#ifdef XRT_BUILD_DRIVER_TWRAP +#include "twrap/twrap_interface.h" +#endif + +#ifdef XRT_BUILD_DRIVER_HANDTRACKING +#include "ht/ht_interface.h" +#endif + +#include "ht_ctrl_emu/ht_ctrl_emu_interface.h" + +#include "xrt/xrt_frameserver.h" +#include "xrt/xrt_results.h" +#include "xrt/xrt_tracking.h" + +#include +#include "math/m_mathinclude.h" + +DEBUG_GET_ONCE_OPTION(ns_config_path, "NS_CONFIG_PATH", NULL) +DEBUG_GET_ONCE_LOG_OPTION(ns_log, "NS_LOG", U_LOGGING_WARN) + + +#define NS_TRACE(...) U_LOG_IFL_T(debug_get_log_option_ns_log(), __VA_ARGS__) +#define NS_DEBUG(...) U_LOG_IFL_D(debug_get_log_option_ns_log(), __VA_ARGS__) +#define NS_INFO(...) U_LOG_IFL_I(debug_get_log_option_ns_log(), __VA_ARGS__) +#define NS_WARN(...) U_LOG_IFL_W(debug_get_log_option_ns_log(), __VA_ARGS__) +#define NS_ERROR(...) U_LOG_IFL_E(debug_get_log_option_ns_log(), __VA_ARGS__) + +static const char *driver_list[] = { + "north_star", +}; + +struct ns_ultraleap_device +{ + bool active; + + // Users input `P_middleofeyes_to_trackingcenter_oxr`, and we invert it into this pose. + // It's a lot simpler to (and everybody does) care about the transform from the eyes center to the device, + // but tracking overrides care about this value. + struct xrt_pose P_trackingcenter_to_middleofeyes_oxr; +}; + +struct ns_depthai_device +{ + bool active; + struct xrt_pose P_imu_to_left_camera_basalt; + struct xrt_pose P_middleofeyes_to_imu_oxr; +}; + +struct ns_t265 +{ + bool active; + struct xrt_pose P_middleofeyes_to_trackingcenter_oxr; +}; + +struct ns_builder +{ + struct xrt_builder base; + + const char *config_path; + cJSON *config_json; + + struct ns_ultraleap_device ultraleap_device; + struct ns_depthai_device depthai_device; + struct ns_t265 t265; +}; + + +static bool +ns_config_load(struct ns_builder *nsb) +{ + const char *file_content = u_file_read_content_from_path(nsb->config_path); + 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.", + nsb->config_path); + return false; + } + + // leaks? + 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", nsb->config_path); + if (error_ptr != NULL) { + U_LOG_E("because of an error before %s", error_ptr); + } + free((void *)file_content); + return false; + } + nsb->config_json = config_json; + free((void *)file_content); + return true; +} + +static void +ns_tracking_config_parse_depthai(struct ns_builder *nsb, bool *out_config_valid) +{ + *out_config_valid = true; + const cJSON *root = u_json_get(nsb->config_json, "depthaiDevice"); + + if (root == NULL) { + *out_config_valid = true; + // not invalid, but doesn't exist. active is not set and won't be used + return; + } + + *out_config_valid = *out_config_valid && // + u_json_get_bool(u_json_get(root, "active"), &nsb->depthai_device.active); + + *out_config_valid = *out_config_valid && // + u_json_get_pose(u_json_get(root, "P_imu_to_left_camera_basalt"), + &nsb->depthai_device.P_imu_to_left_camera_basalt); + + *out_config_valid = *out_config_valid && // + u_json_get_pose(u_json_get(root, "P_middleofeyes_to_imu_oxr"), + &nsb->depthai_device.P_middleofeyes_to_imu_oxr); +} + +static void +ns_tracking_config_parse_ultraleap(struct ns_builder *nsb, bool *out_config_valid) +{ + *out_config_valid = true; + const cJSON *root = u_json_get(nsb->config_json, "leapTracker"); + if (root == NULL) { + // not invalid, but doesn't exist. active is not set and won't be used + return; + } + + struct xrt_pose P_middleofeyes_to_trackingcenter_oxr; + + struct xrt_pose localpose_unity = XRT_POSE_IDENTITY; + + if (u_json_get_pose_permissive(u_json_get(root, "localPose"), &localpose_unity)) { + NS_INFO( + "Found key `localPose` in your Ultraleap tracker config. Converting this from Unity's coordinate " + "space to OpenXR's coordinate space."); + NS_INFO( + "If you just want to specify the offset in OpenXR coordinates, use key " + "`P_middleofeyes_to_trackingcenter` instead."); + + + // This is the conversion from Unity to OpenXR coordinates. + // Unity: X+ Right; Y+ Up; Z+ Forward + // OpenXR: X+ Right; Y+ Up; Z- Forward + // Check tests_quat_change_of_basis to understand the quaternion element negations. + P_middleofeyes_to_trackingcenter_oxr.position.x = localpose_unity.position.x; + P_middleofeyes_to_trackingcenter_oxr.position.y = localpose_unity.position.y; + P_middleofeyes_to_trackingcenter_oxr.position.z = -localpose_unity.position.z; + + + P_middleofeyes_to_trackingcenter_oxr.orientation.x = localpose_unity.orientation.x; + P_middleofeyes_to_trackingcenter_oxr.orientation.y = localpose_unity.orientation.y; + P_middleofeyes_to_trackingcenter_oxr.orientation.z = -localpose_unity.orientation.z; + P_middleofeyes_to_trackingcenter_oxr.orientation.w = -localpose_unity.orientation.w; + + *out_config_valid = *out_config_valid && true; + } else { + *out_config_valid = *out_config_valid && // + u_json_get_pose(u_json_get(root, "P_middleofeyes_to_trackingcenter_oxr"), + &P_middleofeyes_to_trackingcenter_oxr); + } + + math_pose_invert(&P_middleofeyes_to_trackingcenter_oxr, + &nsb->ultraleap_device.P_trackingcenter_to_middleofeyes_oxr); + nsb->ultraleap_device.active = true; +} + +static void +ns_tracking_config_parse_t265(struct ns_builder *nsb, bool *out_config_valid) +{ + *out_config_valid = true; + const cJSON *root = u_json_get(nsb->config_json, "t265"); + + if (root == NULL) { + // not invalid, but doesn't exist. active is not set and won't be used + return; + } + + *out_config_valid = *out_config_valid && // + u_json_get_bool(u_json_get(root, "active"), &nsb->t265.active); + + *out_config_valid = *out_config_valid && // + u_json_get_pose(u_json_get(root, "P_middleofeyes_to_trackingcenter_oxr"), + &nsb->t265.P_middleofeyes_to_trackingcenter_oxr); +} + +void +ns_compute_depthai_ht_offset(struct xrt_pose *P_imu_to_left_camera_basalt, struct xrt_pose *out_pose) +{ + struct xrt_pose deg180 = XRT_POSE_IDENTITY; + + + struct xrt_vec3 plusx = {1, 0, 0}; + struct xrt_vec3 plusz = {0, 0, -1}; + + math_quat_from_plus_x_z(&plusx, &plusz, °180.orientation); + + struct xrt_relation_chain xrc = {0}; + + // Remember, relation_chains are backwards. + // This comes "after" P_imo_to_left_cam_basalt, and rotates from the usual camera coordinate space (+Y down +Z + // forward) to OpenXR/hand tracking's output coordinate space (+Y up +Z backwards) + m_relation_chain_push_pose_if_not_identity(&xrc, °180); + // This comes "first" and goes from the head tracking's output space (IMU) to where the left camera is, + // according to the config file. + m_relation_chain_push_pose_if_not_identity(&xrc, P_imu_to_left_camera_basalt); + + struct xrt_space_relation rel = {0}; + + m_relation_chain_resolve(&xrc, &rel); + + + math_pose_invert(&rel.pose, out_pose); +} + +#ifdef XRT_BUILD_DRIVER_DEPTHAI +static xrt_result_t +ns_setup_depthai_device(struct ns_builder *nsb, + struct u_system_devices *usysd, + struct xrt_device **out_hand_device, + struct xrt_device **out_head_device) +{ + struct depthai_slam_startup_settings settings = {0}; + + settings.frames_per_second = 60; + settings.half_size_ov9282 = true; + settings.want_cameras = true; + settings.want_imu = true; + + struct xrt_fs *the_fs = depthai_fs_slam(&usysd->xfctx, &settings); + + if (the_fs == NULL) { + return XRT_ERROR_DEVICE_CREATION_FAILED; + } + + struct t_stereo_camera_calibration *calib = NULL; + depthai_fs_get_stereo_calibration(the_fs, &calib); + + + + struct xrt_slam_sinks *hand_sinks = NULL; + + struct t_camera_extra_info extra_camera_info; + extra_camera_info.views[0].camera_orientation = CAMERA_ORIENTATION_180; + extra_camera_info.views[1].camera_orientation = CAMERA_ORIENTATION_180; + + extra_camera_info.views[0].boundary_type = HT_IMAGE_BOUNDARY_NONE; + extra_camera_info.views[1].boundary_type = HT_IMAGE_BOUNDARY_NONE; + + int create_status = ht_device_create(&usysd->xfctx, // + calib, // + HT_ALGORITHM_MERCURY, // + extra_camera_info, // + &hand_sinks, // + out_hand_device); + t_stereo_camera_calibration_reference(&calib, NULL); + if (create_status != 0) { + return XRT_ERROR_DEVICE_CREATION_FAILED; + } + + struct xrt_slam_sinks *slam_sinks = NULL; + twrap_slam_create_device(&usysd->xfctx, XRT_DEVICE_DEPTHAI, &slam_sinks, out_head_device); + + struct xrt_slam_sinks entry_sinks = {0}; + struct xrt_frame_sink *entry_left_sink = NULL; + struct xrt_frame_sink *entry_right_sink = NULL; + + u_sink_split_create(&usysd->xfctx, slam_sinks->left, hand_sinks->left, &entry_left_sink); + u_sink_split_create(&usysd->xfctx, slam_sinks->right, hand_sinks->right, &entry_right_sink); + + + entry_sinks = (struct xrt_slam_sinks){ + .left = entry_left_sink, + .right = entry_right_sink, + .imu = slam_sinks->imu, + .gt = slam_sinks->gt, + }; + + struct xrt_slam_sinks dummy_slam_sinks = {0}; + dummy_slam_sinks.imu = entry_sinks.imu; + + u_sink_force_genlock_create(&usysd->xfctx, entry_sinks.left, entry_sinks.right, &dummy_slam_sinks.left, + &dummy_slam_sinks.right); + + xrt_fs_slam_stream_start(the_fs, &dummy_slam_sinks); + + return XRT_SUCCESS; +} +#endif + +// Note: We're just checking for the config file's existence +static xrt_result_t +ns_estimate_system(struct xrt_builder *xb, cJSON *config, struct xrt_prober *xp, struct xrt_builder_estimate *estimate) +{ + struct ns_builder *nsb = (struct ns_builder *)xb; + U_ZERO(estimate); + + nsb->config_path = debug_get_option_ns_config_path(); + + if (nsb->config_path == NULL) { + 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; + } + + estimate->maybe.head = true; + estimate->certain.head = true; + + bool hand_tracking = false; + +#ifdef XRT_BUILD_DRIVER_ULV2 + hand_tracking = + hand_tracking || u_builder_find_prober_device(xpdevs, xpdev_count, ULV2_VID, ULV2_PID, XRT_BUS_TYPE_USB); +#endif + +#ifdef XRT_BUILD_DRIVER_REALSENSE + estimate->certain.dof6 = + estimate->certain.dof6 || u_builder_find_prober_device(xpdevs, xpdev_count, REALSENSE_MOVIDIUS_VID, + REALSENSE_MOVIDIUS_PID, XRT_BUS_TYPE_USB); + estimate->certain.dof6 = + estimate->certain.dof6 || u_builder_find_prober_device(xpdevs, xpdev_count, // + REALSENSE_TM2_VID, REALSENSE_TM2_PID, // + XRT_BUS_TYPE_USB); +#endif + +#ifdef XRT_BUILD_DRIVER_DEPTHAI + bool depthai = u_builder_find_prober_device(xpdevs, xpdev_count, DEPTHAI_VID, DEPTHAI_PID, XRT_BUS_TYPE_USB); +#ifdef XRT_FEATURE_SLAM + estimate->certain.dof6 = estimate->certain.dof6 || depthai; +#endif +#ifdef XRT_BUILD_DRIVER_HANDTRACKING + hand_tracking = hand_tracking || depthai; +#endif +#endif + + estimate->certain.left = estimate->certain.right = estimate->maybe.left = estimate->maybe.right = hand_tracking; + + + + xret = xrt_prober_unlock_list(xp, &xpdevs); + if (xret != XRT_SUCCESS) { + return xret; + } + + return XRT_SUCCESS; +} + + + +static xrt_result_t +ns_open_system(struct xrt_builder *xb, cJSON *config, struct xrt_prober *xp, struct xrt_system_devices **out_xsysd) +{ + struct ns_builder *nsb = (struct ns_builder *)xb; + + + struct u_system_devices *usysd = u_system_devices_allocate(); + xrt_result_t result = XRT_SUCCESS; + + if (out_xsysd == NULL || *out_xsysd != NULL) { + NS_ERROR("Invalid output system pointer"); + result = XRT_ERROR_DEVICE_CREATION_FAILED; + goto end; + } + + + bool load_success = ns_config_load(nsb); + if (!load_success) { + result = XRT_ERROR_DEVICE_CREATION_FAILED; + goto end; + } + + struct xrt_device *ns_hmd = ns_hmd_create(nsb->config_json); + if (ns_hmd == NULL) { + result = XRT_ERROR_DEVICE_CREATION_FAILED; + goto end; + } + + + bool config_valid = true; + ns_tracking_config_parse_depthai(nsb, &config_valid); + if (!config_valid) { + NS_ERROR("DepthAI device config was invalid!"); + } + + ns_tracking_config_parse_ultraleap(nsb, &config_valid); + if (!config_valid) { + NS_ERROR("Leap device config was invalid!"); + } + + ns_tracking_config_parse_t265(nsb, &config_valid); + if (!config_valid) { + NS_ERROR("T265 device config was invalid!"); + } + + struct xrt_device *hand_device = NULL; + struct xrt_device *slam_device = NULL; + + struct xrt_pose head_offset = XRT_POSE_IDENTITY; + + // True if hand tracker is parented to the head tracker (DepthAI), false if hand tracker is parented to + // middle-of-eyes (Ultraleap etc.) + bool hand_parented_to_head_tracker = true; + struct xrt_pose hand_offset = XRT_POSE_IDENTITY; + + // bool got_head_tracker = false; + + + + // For now we use DepthAI for head tracking + hand tracking OR t265 for head + ultraleap for hand. + // Mixing systems with more atomicity coming later™️ + if (nsb->depthai_device.active) { +#ifdef XRT_BUILD_DRIVER_DEPTHAI + NS_INFO("Using DepthAI device!"); + ns_setup_depthai_device(nsb, usysd, &hand_device, &slam_device); + head_offset = nsb->depthai_device.P_middleofeyes_to_imu_oxr; + ns_compute_depthai_ht_offset(&nsb->depthai_device.P_imu_to_left_camera_basalt, &hand_offset); + // got_head_tracker = true; +#else + NS_ERROR("DepthAI head+hand tracker specified in config but DepthAI support was not compiled in!"); +#endif + + + } else { + if (nsb->t265.active) { +#ifdef XRT_BUILD_DRIVER_REALSENSE + slam_device = rs_create_tracked_device_internal_slam(); + head_offset = nsb->t265.P_middleofeyes_to_trackingcenter_oxr; + // got_head_tracker = true; +#else + NS_ERROR( + "Realsense head tracker specified in config but Realsense support was not compiled in!"); +#endif + } + if (nsb->ultraleap_device.active) { +#ifdef XRT_BUILD_DRIVER_ULV2 + ulv2_create_device(&hand_device); + hand_offset = nsb->ultraleap_device.P_trackingcenter_to_middleofeyes_oxr; + hand_parented_to_head_tracker = false; +#else + NS_ERROR( + "Ultraleap hand tracker specified in config but Ultraleap support was not compiled in!"); +#endif + } + } + + + + struct xrt_device *head_wrap = NULL; + + if (slam_device != NULL) { + usysd->base.xdevs[usysd->base.xdev_count++] = slam_device; + head_wrap = multi_create_tracking_override(XRT_TRACKING_OVERRIDE_DIRECT, ns_hmd, slam_device, + XRT_INPUT_GENERIC_TRACKER_POSE, &head_offset); + } else { + // No head tracker, no head tracking. + head_wrap = ns_hmd; + } + + usysd->base.xdevs[usysd->base.xdev_count++] = head_wrap; + usysd->base.roles.head = head_wrap; + + if (hand_device != NULL) { + // note: hand_parented_to_head_tracker is always false when slam_device is NULL + struct xrt_device *hand_wrap = multi_create_tracking_override( + XRT_TRACKING_OVERRIDE_ATTACHED, hand_device, + hand_parented_to_head_tracker ? slam_device : head_wrap, + hand_parented_to_head_tracker ? XRT_INPUT_GENERIC_TRACKER_POSE : XRT_INPUT_GENERIC_HEAD_POSE, + &hand_offset); + struct xrt_device *two_hands[2]; + cemu_devices_create(head_wrap, hand_wrap, two_hands); + + + // usysd->base.xdev_count = 0; + usysd->base.xdevs[usysd->base.xdev_count++] = two_hands[0]; + usysd->base.xdevs[usysd->base.xdev_count++] = two_hands[1]; + + + usysd->base.roles.hand_tracking.left = two_hands[0]; + usysd->base.roles.hand_tracking.right = two_hands[1]; + + usysd->base.roles.left = two_hands[0]; + usysd->base.roles.right = two_hands[1]; + } + + + +end: + if (result == XRT_SUCCESS) { + *out_xsysd = &usysd->base; + } else { + u_system_devices_destroy(&usysd); + } + if (nsb->config_json != NULL) { + cJSON_Delete(nsb->config_json); + } + + return result; +} + +static void +ns_destroy(struct xrt_builder *xb) +{ + free(xb); +} + +/* + * + * 'Exported' functions. + * + */ + +struct xrt_builder * +t_builder_north_star_create(void) +{ + struct ns_builder *sb = U_TYPED_CALLOC(struct ns_builder); + sb->base.estimate_system = ns_estimate_system; + sb->base.open_system = ns_open_system; + sb->base.destroy = ns_destroy; + sb->base.identifier = "north_star"; + sb->base.name = "North Star headset"; + sb->base.driver_identifiers = driver_list; + sb->base.driver_identifier_count = ARRAY_SIZE(driver_list); + + return &sb->base; +} diff --git a/src/xrt/targets/common/target_lists.c b/src/xrt/targets/common/target_lists.c index eca249817..faead9f1f 100644 --- a/src/xrt/targets/common/target_lists.c +++ b/src/xrt/targets/common/target_lists.c @@ -28,10 +28,6 @@ #include "ohmd/oh_interface.h" #endif -#ifdef XRT_BUILD_DRIVER_NS -#include "north_star/ns_interface.h" -#endif - #ifdef XRT_BUILD_DRIVER_PSMV #include "psmv/psmv_interface.h" #endif @@ -108,6 +104,9 @@ xrt_builder_create_func_t target_builder_list[] = { t_builder_remote_create, #endif // T_BUILDER_REMOTE +#ifdef T_BUILDER_NS + t_builder_north_star_create, +#endif #ifdef T_BUILDER_LEGACY t_builder_legacy_create, #endif // T_BUILDER_LEGACY @@ -182,11 +181,6 @@ xrt_auto_prober_create_func_t target_auto_list[] = { oh_create_auto_prober, #endif -#ifdef XRT_BUILD_DRIVER_NS - // North star driver here for now. - ns_create_auto_prober, -#endif - #ifdef XRT_BUILD_DRIVER_ANDROID android_create_auto_prober, #endif