xrt: implement multi device wrappers for tracking overrides

Example config ~/.config/monado/config_v0.json

{
	"active": "tracking",
	"tracking":	{
		"version":	0,
		"tracking_overrides": [
			{
				"target_device_serial": "LHR-E8CC625B",
				"tracker_device_serial": "LHR-1D80A098",
				"offset": {
					"orientation": {
						"x": 0,
						"y": 0,
						"z": 0,
						"w": 1
					},
					"position": {
						"x": 0,
						"y": 0,
						"z": 0
					}
				}
			}
		]
	}
}

v2: Add multi device wrapper

src/xrt/drivers/CMakeLists.txt

@@ -247,6 +247,14 @@ if (XRT_BUILD_DRIVER_ILLIXR)
 	list(APPEND ENABLED_HEADSET_DRIVERS illixr)
 endif()
 
+set(MUlTI_SOURCE_FILES
+	multi_wrapper/multi.c
+	multi_wrapper/multi.h
+	)
+add_library(drv_multi STATIC ${MUlTI_SOURCE_FILES})
+target_link_libraries(drv_multi PUBLIC xrt-interfaces aux_util)
+list(APPEND ENABLED_HEADSET_DRIVERS drv_multi)
+
 if(ENABLED_HEADSET_DRIVERS)
 	set(ENABLED_DRIVERS ${ENABLED_HEADSET_DRIVERS} ${ENABLED_DRIVERS})
 	list(SORT ENABLED_DRIVERS)

src/xrt/drivers/meson.build

@@ -228,3 +228,16 @@ lib_drv_arduino = static_library(
 	dependencies: [dbus, aux],
 	build_by_default: 'arduino' in drivers,
 )
+
+lib_drv_multi = static_library(
+	'drv_multi',
+	files(
+		'multi_wrapper/multi.c',
+		'multi_wrapper/multi.h'
+	),
+	include_directories: [
+		xrt_include,
+	],
+	dependencies: [aux],
+	build_by_default: true,
+)

src/xrt/drivers/multi_wrapper/multi.c

@@ -0,0 +1,158 @@
+// Copyright 2021, Collabora, Ltd.
+// SPDX-License-Identifier: Apache-2.0
+/*!
+ * @file
+ * @brief  Combination of multiple @xrt_device.
+ * @author Christoph Haag <christoph.haag@collabora.com>
+ * @ingroup drv_multi
+ */
+
+#include "multi.h"
+#include "util/u_device.h"
+#include "util/u_debug.h"
+
+#include "math/m_api.h"
+#include "math/m_space.h"
+
+DEBUG_GET_ONCE_LOG_OPTION(multi_log, "MULTI_LOG", U_LOGGING_WARN)
+
+#define MULTI_TRACE(d, ...) U_LOG_XDEV_IFL_T(&d->base, d->ll, __VA_ARGS__)
+#define MULTI_DEBUG(d, ...) U_LOG_XDEV_IFL_D(&d->base, d->ll, __VA_ARGS__)
+#define MULTI_INFO(d, ...) U_LOG_XDEV_IFL_I(&d->base, d->ll, __VA_ARGS__)
+#define MULTI_WARN(d, ...) U_LOG_XDEV_IFL_W(&d->base, d->ll, __VA_ARGS__)
+#define MULTI_ERROR(d, ...) U_LOG_XDEV_IFL_E(&d->base, d->ll, __VA_ARGS__)
+
+struct multi_device
+{
+	struct xrt_device base;
+	enum u_logging_level ll;
+
+	struct
+	{
+		struct xrt_device *target;
+		struct xrt_device *tracker;
+		enum xrt_input_name input_name;
+		struct xrt_pose offset_inv;
+	} tracking_override;
+};
+
+
+static void
+get_tracked_pose(struct xrt_device *xdev,
+                 enum xrt_input_name name,
+                 uint64_t at_timestamp_ns,
+                 struct xrt_space_relation *out_relation)
+{
+	struct multi_device *d = (struct multi_device *)xdev;
+	struct xrt_device *tracker = d->tracking_override.tracker;
+	enum xrt_input_name input_name = d->tracking_override.input_name;
+
+	tracker->get_tracked_pose(tracker, input_name, at_timestamp_ns, out_relation);
+
+	struct xrt_space_graph xsg = {0};
+	m_space_graph_add_pose_if_not_identity(&xsg, &d->tracking_override.offset_inv);
+	m_space_graph_add_relation(&xsg, out_relation);
+	m_space_graph_resolve(&xsg, out_relation);
+}
+
+static void
+destroy(struct xrt_device *xdev)
+{
+	struct multi_device *d = (struct multi_device *)xdev;
+
+	xrt_device_destroy(&d->tracking_override.target);
+
+	// we replaced the target device with us, but no the tracker
+	// xrt_device_destroy(&d->tracking_override.tracker);
+
+	free(d);
+}
+
+static void
+get_hand_tracking(struct xrt_device *xdev,
+                  enum xrt_input_name name,
+                  uint64_t at_timestamp_ns,
+                  struct xrt_hand_joint_set *out_value)
+{
+	struct multi_device *d = (struct multi_device *)xdev;
+	struct xrt_device *target = d->tracking_override.target;
+	xrt_device_get_hand_tracking(target, name, at_timestamp_ns, out_value);
+}
+
+static void
+set_output(struct xrt_device *xdev, enum xrt_output_name name, union xrt_output_value *value)
+{
+	struct multi_device *d = (struct multi_device *)xdev;
+	struct xrt_device *target = d->tracking_override.target;
+	xrt_device_set_output(target, name, value);
+}
+
+static void
+get_view_pose(struct xrt_device *xdev, struct xrt_vec3 *eye_relation, uint32_t view_index, struct xrt_pose *out_pose)
+{
+	struct multi_device *d = (struct multi_device *)xdev;
+	struct xrt_device *target = d->tracking_override.target;
+	xrt_device_get_view_pose(target, eye_relation, view_index, out_pose);
+}
+
+static bool
+compute_distortion(struct xrt_device *xdev, int view, float u, float v, struct xrt_uv_triplet *result)
+{
+	struct multi_device *d = (struct multi_device *)xdev;
+	struct xrt_device *target = d->tracking_override.target;
+	return target->compute_distortion(target, view, u, v, result);
+}
+
+static void
+update_inputs(struct xrt_device *xdev)
+{
+	struct multi_device *d = (struct multi_device *)xdev;
+	struct xrt_device *target = d->tracking_override.target;
+	xrt_device_update_inputs(target);
+}
+
+
+struct xrt_device *
+multi_create_tracking_override(struct xrt_device *tracking_override_target,
+                               struct xrt_device *tracking_override_tracker,
+                               enum xrt_input_name tracking_override_input_name,
+                               struct xrt_pose *offset)
+{
+	struct multi_device *d = U_TYPED_CALLOC(struct multi_device);
+
+	if (!d) {
+		return NULL;
+	}
+
+	d->ll = debug_get_log_option_multi_log();
+
+	// mimic the tracking override target
+	d->base = *tracking_override_target;
+
+	// but take orientation and position tracking capabilities from tracker
+	d->base.orientation_tracking_supported = tracking_override_target->orientation_tracking_supported;
+	d->base.position_tracking_supported = tracking_override_target->position_tracking_supported;
+
+	// because we use the tracking data of the tracker, we use its tracking origin instead
+	d->base.tracking_origin = tracking_override_tracker->tracking_origin;
+
+	// The offset describes the physical pose of the tracker in the space of the thing we want to track.
+	// For a tracker that is physically attached at y=.1m to the tracked thing, when querying the pose for the
+	// tracked thing, we want to transform its pose by y-=.1m relative to the tracker. Multiple target devices may
+	// share a single tracker, therefore we can not simply adjust the tracker's tracking origin.
+	math_pose_invert(offset, &d->tracking_override.offset_inv);
+
+	d->tracking_override.target = tracking_override_target;
+	d->tracking_override.tracker = tracking_override_tracker;
+	d->tracking_override.input_name = tracking_override_input_name;
+
+	d->base.get_tracked_pose = get_tracked_pose;
+	d->base.destroy = destroy;
+	d->base.get_hand_tracking = get_hand_tracking;
+	d->base.set_output = set_output;
+	d->base.update_inputs = update_inputs;
+	d->base.compute_distortion = compute_distortion;
+	d->base.get_view_pose = get_view_pose;
+
+	return &d->base;
+}

src/xrt/drivers/multi_wrapper/multi.h

@@ -0,0 +1,51 @@
+// Copyright 2021, Collabora, Ltd.
+// SPDX-License-Identifier: Apache-2.0
+/*!
+ * @file
+ * @brief  Combination of multiple @xrt_device.
+ * @author Christoph Haag <christoph.haag@collabora.com>
+ * @ingroup drv_multi
+ */
+
+#pragma once
+
+#include "xrt/xrt_defines.h"
+#include "xrt/xrt_device.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/*!
+ * @defgroup drv_multi Multi device wrapper driver
+ * @ingroup drv
+ *
+ * @brief Driver that can wrap multiple devices, for example to override tracking.
+ */
+
+/*!
+ * Create a device that takes ownership of the target device and mimics it.
+ *
+ * Does not take ownership of the tracker device, one can be assigned to multiple targets.
+ *
+ * The pose provided by get_tracked_pose will be provided by the tracker device.
+ *
+ * @param tracking_override_target An existing device that will be mimiced by the created device.
+ * @param tracking_override_tracker An existing device that will be used to provide tracking data.
+ * @param tracking_override_input_name The input name of the tracker device. XRT_INPUT_GENERIC_TRACKER_POSE for generic
+ * trackers.
+ * @param offset A static offset describing the real world transform from the "tracked point" of the target device to
+ * the "tracked point" of the tracker device. A tracking sensors attached .1m above the HMD "center" sets y = 0.1.
+ *
+ * @ingroup drv_multi
+ */
+struct xrt_device *
+multi_create_tracking_override(struct xrt_device *tracking_override_target,
+                               struct xrt_device *tracking_override_tracker,
+                               enum xrt_input_name tracking_override_input_name,
+                               struct xrt_pose *offset);
+
+#ifdef __cplusplus
+}
+#endif

src/xrt/include/xrt/xrt_settings.h

@@ -36,6 +36,16 @@ enum xrt_settings_camera_type
 #define XRT_SETTINGS_CAMERA_NAME_LENGTH 256
 #define XRT_SETTINGS_PATH_LENGTH 1024
 
+#define XRT_MAX_TRACKING_OVERRIDES 16
+
+struct xrt_tracking_override
+{
+	char target_device_serial[XRT_DEVICE_NAME_LEN];
+	char tracker_device_serial[XRT_DEVICE_NAME_LEN];
+	enum xrt_input_name input_name;
+	struct xrt_pose offset;
+};
+
 /*!
  * Holding enough information to recreate a tracking pipeline.
  */

src/xrt/state_trackers/oxr/oxr_xdev.c

@@ -90,6 +90,7 @@ oxr_xdev_get_space_graph(struct oxr_logger *log,
 	uint64_t at_timestamp_ns = time_state_ts_to_monotonic_ns(inst->timekeeping, at_time);
 
 	struct xrt_space_relation *rel = m_space_graph_reserve(xsg);
+
 	xrt_device_get_tracked_pose(xdev, name, at_timestamp_ns, rel);
 
 	// Add in the offset from the tracking system.

src/xrt/state_trackers/prober/p_json.c

@@ -134,6 +134,22 @@ get_obj_int(cJSON *json, const char *name, int *out_int)
 	return true;
 }
 
+static bool
+get_obj_float(cJSON *json, const char *name, float *out_float)
+{
+	cJSON *item = get_obj(json, name);
+	if (item == NULL) {
+		return false;
+	}
+
+	if (!u_json_get_float(item, out_float)) {
+		U_LOG_E("Failed to parse '%s'!", name);
+		return false;
+	}
+
+	return true;
+}
+
 static bool
 get_obj_str(cJSON *json, const char *name, char *array, size_t array_size)
 {
@@ -229,8 +245,8 @@ p_json_get_remote_port(struct prober *p, int *out_port)
 	return true;
 }
 
-bool
-p_json_get_tracking_settings(struct prober *p, struct xrt_settings_tracking *s)
+static cJSON *
+open_tracking_settings(struct prober *p)
 {
 	if (p->json.root == NULL) {
 		if (p->json.file_loaded) {
@@ -238,26 +254,90 @@ p_json_get_tracking_settings(struct prober *p, struct xrt_settings_tracking *s)
 		} else {
 			U_LOG_W("No config file!");
 		}
-		return false;
+		return NULL;
 	}
 
 	cJSON *t = cJSON_GetObjectItemCaseSensitive(p->json.root, "tracking");
 	if (t == NULL) {
 		U_LOG_E("No tracking node");
-		return false;
+		return NULL;
 	}
 
-	char tmp[16];
-
 	int ver = -1;
 	bool bad = false;
 
 	bad |= !get_obj_int(t, "version", &ver);
 	if (bad || ver >= 1) {
 		U_LOG_E("Missing or unknown version  tag '%i'", ver);
+		return NULL;
+	}
+
+	return t;
+}
+
+bool
+p_json_get_tracking_overrides(struct prober *p, struct xrt_tracking_override *out_overrides, size_t *out_num_overrides)
+{
+	cJSON *t = open_tracking_settings(p);
+	if (t == NULL) {
+		U_LOG_E("No tracking node");
 		return false;
 	}
 
+
+	cJSON *overrides = cJSON_GetObjectItemCaseSensitive(t, "tracking_overrides");
+
+	*out_num_overrides = 0;
+
+	cJSON *override = NULL;
+	cJSON_ArrayForEach(override, overrides)
+	{
+		bool bad = false;
+
+		struct xrt_tracking_override *o = &out_overrides[(*out_num_overrides)++];
+		bad |= !get_obj_str(override, "target_device_serial", o->target_device_serial, XRT_DEVICE_NAME_LEN);
+		bad |= !get_obj_str(override, "tracker_device_serial", o->tracker_device_serial, XRT_DEVICE_NAME_LEN);
+
+		cJSON *offset = cJSON_GetObjectItemCaseSensitive(override, "offset");
+		if (offset) {
+			cJSON *orientation = cJSON_GetObjectItemCaseSensitive(offset, "orientation");
+			bad |= !get_obj_float(orientation, "x", &o->offset.orientation.x);
+			bad |= !get_obj_float(orientation, "y", &o->offset.orientation.y);
+			bad |= !get_obj_float(orientation, "z", &o->offset.orientation.z);
+			bad |= !get_obj_float(orientation, "w", &o->offset.orientation.w);
+
+			cJSON *position = cJSON_GetObjectItemCaseSensitive(offset, "position");
+			bad |= !get_obj_float(position, "x", &o->offset.position.x);
+			bad |= !get_obj_float(position, "y", &o->offset.position.y);
+			bad |= !get_obj_float(position, "z", &o->offset.position.z);
+		} else {
+			o->offset.orientation.w = 1;
+		}
+
+		//! @todo support arbitrary tracking inputs for overrides
+		o->input_name = XRT_INPUT_GENERIC_TRACKER_POSE;
+
+		if (bad) {
+			*out_num_overrides = 0;
+			return false;
+		}
+	}
+	return true;
+}
+
+bool
+p_json_get_tracking_settings(struct prober *p, struct xrt_settings_tracking *s)
+{
+	cJSON *t = open_tracking_settings(p);
+	if (t == NULL) {
+		U_LOG_E("No tracking node");
+		return false;
+	}
+
+	char tmp[16];
+
+	bool bad = false;
+
 	bad |= !get_obj_str(t, "camera_name", s->camera_name, sizeof(s->camera_name));
 	bad |= !get_obj_int(t, "camera_mode", &s->camera_mode);
 	bad |= !get_obj_str(t, "camera_type", tmp, sizeof(tmp));

src/xrt/state_trackers/prober/p_prober.c

@@ -32,6 +32,7 @@
 #include <string.h>
 #include <assert.h>
 
+#include "multi_wrapper/multi.h"
 
 /*
  *
@@ -770,6 +771,51 @@ add_from_remote(struct prober *p, struct xrt_device **xdevs, size_t num_xdevs, b
 #endif
 }
 
+static void
+apply_tracking_override(struct prober *p, struct xrt_device **xdevs, size_t num_xdevs, struct xrt_tracking_override *o)
+{
+	struct xrt_device *target_xdev = NULL;
+	size_t target_idx = 0;
+	struct xrt_device *tracker_xdev = NULL;
+
+	for (size_t i = 0; i < num_xdevs; i++) {
+		struct xrt_device *xdev = xdevs[i];
+		if (xdev == NULL) {
+			continue;
+		}
+
+		if (strncmp(xdev->serial, o->target_device_serial, XRT_DEVICE_NAME_LEN) == 0) {
+			target_xdev = xdev;
+			target_idx = i;
+		}
+		if (strncmp(xdev->serial, o->tracker_device_serial, XRT_DEVICE_NAME_LEN) == 0) {
+			tracker_xdev = xdev;
+		}
+	}
+
+	if (target_xdev == NULL) {
+		P_ERROR(p, "Tracking override target xdev %s not found", o->target_device_serial);
+	}
+
+	if (tracker_xdev == NULL) {
+		P_ERROR(p, "Tracking override tracker xdev %s not found", o->tracker_device_serial);
+	}
+
+
+	if (target_xdev != NULL && tracker_xdev != NULL) {
+		struct xrt_device *multi =
+		    multi_create_tracking_override(target_xdev, tracker_xdev, o->input_name, &o->offset);
+
+		if (multi) {
+			// drops the target device from the list, but keeps the tracker
+			// a tracker could be attached to multiple targets with different names
+			xdevs[target_idx] = multi;
+		} else {
+			P_ERROR(p, "Failed to create tracking override multi device");
+		}
+	}
+}
+
 static int
 select_device(struct xrt_prober *xp, struct xrt_device **xdevs, size_t num_xdevs)
 {
@@ -808,6 +854,15 @@ select_device(struct xrt_prober *xp, struct xrt_device **xdevs, size_t num_xdevs
 		break;
 	}
 
+	struct xrt_tracking_override overrides[XRT_MAX_TRACKING_OVERRIDES];
+	size_t num_overrides = 0;
+	if (p_json_get_tracking_overrides(p, overrides, &num_overrides)) {
+		for (size_t i = 0; i < num_overrides; i++) {
+			struct xrt_tracking_override *o = &overrides[i];
+			apply_tracking_override(p, xdevs, num_xdevs, o);
+		}
+	}
+
 	if (have_hmd) {
 		P_DEBUG(p, "Found HMD! '%s'", xdevs[0]->str);
 		return 0;

src/xrt/state_trackers/prober/p_prober.h

@@ -205,6 +205,17 @@ p_json_get_active(struct prober *p, enum p_active_config *out_active);
 bool
 p_json_get_tracking_settings(struct prober *p, struct xrt_settings_tracking *s);
 
+/*!
+ * Extract tracking override settings from the JSON.
+ *
+ * Caller allocates an array of XRT_MAX_TRACKING_OVERRIDES tracking_override.
+ *
+ * @public @memberof prober
+ * @relatesalso xrt_settings_tracking
+ */
+bool
+p_json_get_tracking_overrides(struct prober *p, struct xrt_tracking_override *out_overrides, size_t *out_num_overrides);
+
 /*!
  * Extract remote settings from the JSON.
  *

src/xrt/targets/common/CMakeLists.txt

@@ -92,6 +92,8 @@ if(XRT_BUILD_DRIVER_ILLIXR)
 	target_link_libraries(target_lists PRIVATE drv_illixr)
 endif()
 
+target_link_libraries(target_lists PRIVATE drv_multi)
+
 ####
 # Instance
 #

src/xrt/targets/meson.build

@@ -77,6 +77,8 @@ if 'remote' in drivers
 	driver_libs += [lib_drv_remote]
 endif
 
+driver_libs += [lib_drv_multi]
+
 subdir('common')
 subdir('openxr')
 subdir('cli')