d/sample: Clean up sample HMD, use m_relation_history, populate mesh

src/xrt/drivers/sample/sample_hmd.c

@@ -1,4 +1,4 @@
-// Copyright 2020-2021, Collabora, Ltd.
+// Copyright 2020-2024, Collabora, Ltd.
 // SPDX-License-Identifier: BSL-1.0
 /*!
  * @file
@@ -12,17 +12,21 @@
  * @ingroup drv_sample
  */
 
+#include "os/os_time.h"
+#include "xrt/xrt_defines.h"
 #include "xrt/xrt_device.h"
 
+#include "math/m_relation_history.h"
 #include "math/m_api.h"
 #include "math/m_mathinclude.h" // IWYU pragma: keep
 
-#include "util/u_var.h"
-#include "util/u_time.h"
 #include "util/u_debug.h"
 #include "util/u_device.h"
-#include "util/u_logging.h"
 #include "util/u_distortion_mesh.h"
+#include "util/u_logging.h"
+#include "util/u_misc.h"
+#include "util/u_time.h"
+#include "util/u_var.h"
 #include "util/u_visibility_mask.h"
 #include "xrt/xrt_results.h"
 
@@ -47,6 +51,9 @@ struct sample_hmd
 	struct xrt_pose pose;
 
 	enum u_logging_level log_level;
+
+	// has built-in mutex so thread safe
+	struct m_relation_history *relation_hist;
 };
 
 
@@ -59,26 +66,30 @@ sample_hmd(struct xrt_device *xdev)
 
 DEBUG_GET_ONCE_LOG_OPTION(sample_log, "SAMPLE_LOG", U_LOGGING_WARN)
 
-#define SH_TRACE(p, ...) U_LOG_XDEV_IFL_T(&sh->base, sh->log_level, __VA_ARGS__)
-#define SH_DEBUG(p, ...) U_LOG_XDEV_IFL_D(&sh->base, sh->log_level, __VA_ARGS__)
-#define SH_ERROR(p, ...) U_LOG_XDEV_IFL_E(&sh->base, sh->log_level, __VA_ARGS__)
+#define HMD_TRACE(hmd, ...) U_LOG_XDEV_IFL_T(&hmd->base, hmd->log_level, __VA_ARGS__)
+#define HMD_DEBUG(hmd, ...) U_LOG_XDEV_IFL_D(&hmd->base, hmd->log_level, __VA_ARGS__)
+#define HMD_INFO(hmd, ...) U_LOG_XDEV_IFL_I(&hmd->base, hmd->log_level, __VA_ARGS__)
+#define HMD_ERROR(hmd, ...) U_LOG_XDEV_IFL_E(&hmd->base, hmd->log_level, __VA_ARGS__)
 
 static void
 sample_hmd_destroy(struct xrt_device *xdev)
 {
-	struct sample_hmd *sh = sample_hmd(xdev);
+	struct sample_hmd *hmd = sample_hmd(xdev);
 
 	// Remove the variable tracking.
-	u_var_remove_root(sh);
+	u_var_remove_root(hmd);
 
-	u_device_free(&sh->base);
+
+	m_relation_history_destroy(&hmd->relation_hist);
+
+	u_device_free(&hmd->base);
 }
 
 static void
 sample_hmd_update_inputs(struct xrt_device *xdev)
 {
 	/*
-	 * Empty for the sampler driver, if you need to you should
+	 * Empty for the sample driver, if you need to you should
 	 * put code to update the attached inputs fields. If not you can use
 	 * the u_device_noop_update_inputs helper to make it a no-op.
 	 */
@@ -90,19 +101,29 @@ sample_hmd_get_tracked_pose(struct xrt_device *xdev,
                             uint64_t at_timestamp_ns,
                             struct xrt_space_relation *out_relation)
 {
-	struct sample_hmd *sh = sample_hmd(xdev);
+	struct sample_hmd *hmd = sample_hmd(xdev);
 
 	if (name != XRT_INPUT_GENERIC_HEAD_POSE) {
-		SH_ERROR(sh, "unknown input name");
+		HMD_ERROR(hmd, "unknown input name");
 		return;
 	}
 
-	// Estimate pose at timestamp at_timestamp_ns!
-	math_quat_normalize(&sh->pose.orientation);
-	out_relation->pose = sh->pose;
-	out_relation->relation_flags = (enum xrt_space_relation_flags)(XRT_SPACE_RELATION_ORIENTATION_VALID_BIT |
-	                                                               XRT_SPACE_RELATION_POSITION_VALID_BIT |
-	                                                               XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT);
+	struct xrt_space_relation relation = XRT_SPACE_RELATION_ZERO;
+
+	enum m_relation_history_result history_result =
+	    m_relation_history_get(hmd->relation_hist, at_timestamp_ns, &relation);
+	if (history_result == M_RELATION_HISTORY_RESULT_INVALID) {
+		// If you get in here, it means you did not push any poses into the relation history.
+		// You may want to handle this differently.
+		HMD_ERROR(hmd, "Internal error: no poses pushed?");
+	}
+
+	if ((relation.relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) != 0) {
+		// If we provide an orientation, make sure that it is normalized.
+		math_quat_normalize(&relation.pose.orientation);
+	}
+
+	*out_relation = relation;
 }
 
 static void
@@ -128,13 +149,6 @@ sample_hmd_get_view_poses(struct xrt_device *xdev,
 	    out_poses);           //
 }
 
-bool
-sample_hmd_compute_distortion(
-    struct xrt_device *xdev, uint32_t view, float u, float v, struct xrt_uv_triplet *out_result)
-{
-	return u_compute_distortion_none(u, u, out_result);
-}
-
 xrt_result_t
 sample_hmd_get_visibility_mask(struct xrt_device *xdev,
                                enum xrt_visibility_mask_type type,
@@ -153,37 +167,44 @@ sample_hmd_create(void)
 	enum u_device_alloc_flags flags =
 	    (enum u_device_alloc_flags)(U_DEVICE_ALLOC_HMD | U_DEVICE_ALLOC_TRACKING_NONE);
 
-	struct sample_hmd *sh = U_DEVICE_ALLOCATE(struct sample_hmd, flags, 1, 0);
+	struct sample_hmd *hmd = U_DEVICE_ALLOCATE(struct sample_hmd, flags, 1, 0);
 
 	// This list should be ordered, most preferred first.
 	size_t idx = 0;
-	sh->base.hmd->blend_modes[idx++] = XRT_BLEND_MODE_OPAQUE;
-	sh->base.hmd->blend_mode_count = idx;
+	hmd->base.hmd->blend_modes[idx++] = XRT_BLEND_MODE_OPAQUE;
+	hmd->base.hmd->blend_mode_count = idx;
 
-	sh->base.update_inputs = sample_hmd_update_inputs;
-	sh->base.get_tracked_pose = sample_hmd_get_tracked_pose;
-	sh->base.get_view_poses = sample_hmd_get_view_poses;
-	sh->base.compute_distortion = sample_hmd_compute_distortion;
-	sh->base.get_visibility_mask = sample_hmd_get_visibility_mask;
-	sh->base.destroy = sample_hmd_destroy;
+	hmd->base.update_inputs = sample_hmd_update_inputs;
+	hmd->base.get_tracked_pose = sample_hmd_get_tracked_pose;
+	hmd->base.get_view_poses = sample_hmd_get_view_poses;
+	hmd->base.get_visibility_mask = sample_hmd_get_visibility_mask;
+	hmd->base.destroy = sample_hmd_destroy;
 
-	sh->pose = (struct xrt_pose)XRT_POSE_IDENTITY;
-	sh->log_level = debug_get_log_option_sample_log();
+	// Distortion information, fills in xdev->compute_distortion().
+	u_distortion_mesh_set_none(&hmd->base);
+
+	// populate this with something more complex if required
+	// hmd->base.compute_distortion = sample_hmd_compute_distortion;
+
+	hmd->pose = (struct xrt_pose)XRT_POSE_IDENTITY;
+	hmd->log_level = debug_get_log_option_sample_log();
 
 	// Print name.
-	snprintf(sh->base.str, XRT_DEVICE_NAME_LEN, "Sample HMD");
-	snprintf(sh->base.serial, XRT_DEVICE_NAME_LEN, "Sample HMD S/N");
+	snprintf(hmd->base.str, XRT_DEVICE_NAME_LEN, "Sample HMD");
+	snprintf(hmd->base.serial, XRT_DEVICE_NAME_LEN, "Sample HMD S/N");
+
+	m_relation_history_create(&hmd->relation_hist);
 
 	// Setup input.
-	sh->base.name = XRT_DEVICE_GENERIC_HMD;
-	sh->base.device_type = XRT_DEVICE_TYPE_HMD;
-	sh->base.inputs[0].name = XRT_INPUT_GENERIC_HEAD_POSE;
-	sh->base.orientation_tracking_supported = true;
-	sh->base.position_tracking_supported = false;
+	hmd->base.name = XRT_DEVICE_GENERIC_HMD;
+	hmd->base.device_type = XRT_DEVICE_TYPE_HMD;
+	hmd->base.inputs[0].name = XRT_INPUT_GENERIC_HEAD_POSE;
+	hmd->base.orientation_tracking_supported = true;
+	hmd->base.position_tracking_supported = true;
 
 	// Set up display details
 	// refresh rate
-	sh->base.hmd->screens[0].nominal_frame_interval_ns = time_s_to_ns(1.0f / 90.0f);
+	hmd->base.hmd->screens[0].nominal_frame_interval_ns = time_s_to_ns(1.0f / 90.0f);
 
 	const double hFOV = 90 * (M_PI / 180.0);
 	const double vFOV = 96.73 * (M_PI / 180.0);
@@ -192,46 +213,52 @@ sample_hmd_create(void)
 	const double vCOP = 0.5;
 	if (
 	    /* right eye */
-	    !math_compute_fovs(1, hCOP, hFOV, 1, vCOP, vFOV, &sh->base.hmd->distortion.fov[1]) ||
+	    !math_compute_fovs(1, hCOP, hFOV, 1, vCOP, vFOV, &hmd->base.hmd->distortion.fov[1]) ||
 	    /*
 	     * left eye - same as right eye, except the horizontal center of projection is moved in the opposite
 	     * direction now
 	     */
-	    !math_compute_fovs(1, 1.0 - hCOP, hFOV, 1, vCOP, vFOV, &sh->base.hmd->distortion.fov[0])) {
+	    !math_compute_fovs(1, 1.0 - hCOP, hFOV, 1, vCOP, vFOV, &hmd->base.hmd->distortion.fov[0])) {
 		// If those failed, it means our math was impossible.
-		SH_ERROR(sh, "Failed to setup basic device info");
-		sample_hmd_destroy(&sh->base);
+		HMD_ERROR(hmd, "Failed to setup basic device info");
+		sample_hmd_destroy(&hmd->base);
 		return NULL;
 	}
 	const int panel_w = 1080;
 	const int panel_h = 1200;
 
 	// Single "screen" (always the case)
-	sh->base.hmd->screens[0].w_pixels = panel_w * 2;
-	sh->base.hmd->screens[0].h_pixels = panel_h;
+	hmd->base.hmd->screens[0].w_pixels = panel_w * 2;
+	hmd->base.hmd->screens[0].h_pixels = panel_h;
 
 	// Left, Right
 	for (uint8_t eye = 0; eye < 2; ++eye) {
-		sh->base.hmd->views[eye].display.w_pixels = panel_w;
-		sh->base.hmd->views[eye].display.h_pixels = panel_h;
-		sh->base.hmd->views[eye].viewport.y_pixels = 0;
-		sh->base.hmd->views[eye].viewport.w_pixels = panel_w;
-		sh->base.hmd->views[eye].viewport.h_pixels = panel_h;
+		hmd->base.hmd->views[eye].display.w_pixels = panel_w;
+		hmd->base.hmd->views[eye].display.h_pixels = panel_h;
+		hmd->base.hmd->views[eye].viewport.y_pixels = 0;
+		hmd->base.hmd->views[eye].viewport.w_pixels = panel_w;
+		hmd->base.hmd->views[eye].viewport.h_pixels = panel_h;
 		// if rotation is not identity, the dimensions can get more complex.
-		sh->base.hmd->views[eye].rot = u_device_rotation_ident;
+		hmd->base.hmd->views[eye].rot = u_device_rotation_ident;
 	}
 	// left eye starts at x=0, right eye starts at x=panel_width
-	sh->base.hmd->views[0].viewport.x_pixels = 0;
-	sh->base.hmd->views[1].viewport.x_pixels = panel_w;
+	hmd->base.hmd->views[0].viewport.x_pixels = 0;
+	hmd->base.hmd->views[1].viewport.x_pixels = panel_w;
 
 	// Distortion information, fills in xdev->compute_distortion().
-	u_distortion_mesh_set_none(&sh->base);
+	u_distortion_mesh_set_none(&hmd->base);
+
+	// Just put an initial identity value in the tracker
+	struct xrt_space_relation identity = XRT_SPACE_RELATION_ZERO;
+	identity.relation_flags = (enum xrt_space_relation_flags)(XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT |
+	                                                          XRT_SPACE_RELATION_ORIENTATION_VALID_BIT);
+	uint64_t now = os_monotonic_get_ns();
+	m_relation_history_push(hmd->relation_hist, &identity, now);
 
 	// Setup variable tracker: Optional but useful for debugging
-	u_var_add_root(sh, "Sample HMD", true);
-	u_var_add_pose(sh, &sh->pose, "pose");
-	u_var_add_log_level(sh, &sh->log_level, "log_level");
+	u_var_add_root(hmd, "Sample HMD", true);
+	u_var_add_log_level(hmd, &hmd->log_level, "log_level");
 
 
-	return &sh->base;
+	return &hmd->base;
 }

src/xrt/drivers/sample/sample_interface.h

@@ -1,8 +1,8 @@
-// Copyright 2020-2021, Collabora, Ltd.
+// Copyright 2020-2024, Collabora, Ltd.
 // SPDX-License-Identifier: BSL-1.0
 /*!
  * @file
- * @brief  Interface to sample driver.
+ * @brief  Interface to Sample HMD driver.
  * @author Jakob Bornecrantz <jakob@collabora.com>
  * @author Rylie Pavlik <rylie.pavlik@collabora.com>
  * @ingroup drv_sample
@@ -15,10 +15,13 @@ extern "C" {
 #endif
 
 /*!
- * @defgroup drv_sample Sample driver
+ * @defgroup drv_sample Sample HMD driver
  * @ingroup drv
  *
- * @brief Simple do-nothing sample driver, that cannot be detected by USB VID/PID
+ * @brief Driver for a Sample HMD.
+ *
+ * Does no actual work.
+ * Assumed to not be detectable by USB VID/PID,
  * and thus exposes an "auto-prober" to explicitly discover the device.
  *
  * See @ref writing-driver for additional information.
@@ -30,10 +33,11 @@ extern "C" {
  * you can skip the @ref xrt_auto_prober implementation, and instead implement a
  * "found" function that matches the signature expected by xrt_prober_entry::found.
  * See for example @ref hdk_found.
+ * Alternately, you might create a builder or an instance implementation directly.
  */
 
 /*!
- * Create a auto prober for a sample device.
+ * Create a auto prober for a Sample HMD.
  *
  * @ingroup drv_sample
  */
@@ -41,10 +45,10 @@ struct xrt_auto_prober *
 sample_create_auto_prober(void);
 
 /*!
- * Create a sample hmd.
+ * Create a Sample HMD.
  *
  * This is only exposed so that the prober (in one source file)
- * can call the construction function (in another.)
+ * can call the construction function (in another)
  * @ingroup drv_sample
  */
 struct xrt_device *

src/xrt/drivers/sample/sample_prober.c

@@ -1,8 +1,8 @@
-// Copyright 2020-2021, Collabora, Ltd.
+// Copyright 2020-2024, Collabora, Ltd.
 // SPDX-License-Identifier: BSL-1.0
 /*!
  * @file
- * @brief  Sample prober code.
+ * @brief  "auto-prober" for Sample HMD that can be autodetected but not through USB VID/PID.
  * @author Jakob Bornecrantz <jakob@collabora.com>
  * @ingroup drv_sample
  */
@@ -10,7 +10,6 @@
 #include "xrt/xrt_prober.h"
 
 #include "util/u_misc.h"
-#include "util/u_debug.h"
 
 #include "sample_interface.h"
 
@@ -25,18 +24,18 @@ struct sample_auto_prober
 
 //! @private @memberof sample_auto_prober
 static inline struct sample_auto_prober *
-sample_auto_prober(struct xrt_auto_prober *p)
+sample_auto_prober(struct xrt_auto_prober *xap)
 {
-	return (struct sample_auto_prober *)p;
+	return (struct sample_auto_prober *)xap;
 }
 
 //! @private @memberof sample_auto_prober
 static void
 sample_auto_prober_destroy(struct xrt_auto_prober *p)
 {
-	struct sample_auto_prober *sap = sample_auto_prober(p);
+	struct sample_auto_prober *ap = sample_auto_prober(p);
 
-	free(sap);
+	free(ap);
 }
 
 //! @public @memberof sample_auto_prober
@@ -47,10 +46,10 @@ sample_auto_prober_autoprobe(struct xrt_auto_prober *xap,
                              struct xrt_prober *xp,
                              struct xrt_device **out_xdevs)
 {
-	struct sample_auto_prober *sap = sample_auto_prober(xap);
-	(void)sap;
+	struct sample_auto_prober *ap = sample_auto_prober(xap);
+	(void)ap;
 
-	// Do not create a sample HMD if we are not looking for HMDs.
+	// Do not create an HMD device if we are not looking for HMDs.
 	if (no_hmds) {
 		return 0;
 	}
@@ -62,10 +61,10 @@ sample_auto_prober_autoprobe(struct xrt_auto_prober *xap,
 struct xrt_auto_prober *
 sample_create_auto_prober(void)
 {
-	struct sample_auto_prober *sap = U_TYPED_CALLOC(struct sample_auto_prober);
-	sap->base.name = "Sample";
-	sap->base.destroy = sample_auto_prober_destroy;
-	sap->base.lelo_dallas_autoprobe = sample_auto_prober_autoprobe;
+	struct sample_auto_prober *ap = U_TYPED_CALLOC(struct sample_auto_prober);
+	ap->base.name = "Sample HMD Auto-Prober";
+	ap->base.destroy = sample_auto_prober_destroy;
+	ap->base.lelo_dallas_autoprobe = sample_auto_prober_autoprobe;
 
-	return &sap->base;
+	return &ap->base;
 }