d/rs: Implement new probing logic to choose between ddev and hdev creation

Also adds a rs_container to keep track of RealSense created objects that
are useful to have during execution.

src/xrt/drivers/CMakeLists.txt

@@ -180,6 +180,7 @@ if(XRT_BUILD_DRIVER_RS)
 	set(RS_SOURCE_FILES
 		realsense/rs_ddev.c
 		realsense/rs_prober.c
+		realsense/rs_driver.h
 		realsense/rs_interface.h
 		)
 

src/xrt/drivers/meson.build

@@ -144,6 +144,7 @@ lib_drv_rs = static_library(
 	files(
 		'realsense/rs_ddev.c',
 		'realsense/rs_interface.h',
+		'realsense/rs_driver.h',
 		'realsense/rs_prober.c',
 	),
 	include_directories: [xrt_include,cjson_include],

src/xrt/drivers/realsense/rs_ddev.c

@@ -27,6 +27,8 @@
 #include "util/u_json.h"
 #include "util/u_config_json.h"
 
+#include "rs_driver.h"
+
 #include <librealsense2/rs.h>
 #include <librealsense2/h/rs_pipeline.h>
 #include <librealsense2/h/rs_option.h>
@@ -64,10 +66,7 @@ struct rs_ddev
 	bool enable_pose_prediction;
 	bool enable_pose_filtering; //!< Forward compatibility for when that 1-euro filter is working
 
-	rs2_context *ctx;
-	rs2_pipeline *pipe;
-	rs2_pipeline_profile *profile;
-	rs2_config *config;
+	struct rs_container rsc; //!< Container of realsense API related objects
 };
 
 
@@ -102,26 +101,9 @@ check_error(struct rs_ddev *rs, rs2_error *e)
 static void
 close_ddev(struct rs_ddev *rs)
 {
-	if (rs->config) {
-		rs2_delete_config(rs->config);
-		rs->config = NULL;
-	}
-
-	if (rs->profile) {
-		rs2_delete_pipeline_profile(rs->profile);
-		rs->profile = NULL;
-	}
-
-	if (rs->pipe) {
-		rs2_pipeline_stop(rs->pipe, NULL);
-		rs2_delete_pipeline(rs->pipe);
-		rs->pipe = NULL;
-	}
-
-	if (rs->ctx) {
-		rs2_delete_context(rs->ctx);
-		rs->ctx = NULL;
-	}
+	struct rs_container *rsc = &rs->rsc;
+	rs2_pipeline_stop(rsc->pipeline, NULL);
+	rs_container_cleanup(&rs->rsc);
 }
 
 #define CHECK_RS2()                                                                                                    \
@@ -137,35 +119,48 @@ close_ddev(struct rs_ddev *rs)
  * Create all RealSense resources needed for 6DOF tracking.
  */
 static int
-create_ddev(struct rs_ddev *rs)
+create_ddev(struct rs_ddev *rs, int device_idx)
 {
 	assert(rs != NULL);
 	rs2_error *e = NULL;
 
-	rs->ctx = rs2_create_context(RS2_API_VERSION, &e);
+	struct rs_container *rsc = &rs->rsc;
+
+	rsc->context = rs2_create_context(RS2_API_VERSION, &e);
 	CHECK_RS2();
 
-	rs2_device_list *device_list = rs2_query_devices(rs->ctx, &e);
+	rsc->device_list = rs2_query_devices(rsc->context, &e);
 	CHECK_RS2();
 
-	int dev_count = rs2_get_device_count(device_list, &e);
+	rsc->pipeline = rs2_create_pipeline(rsc->context, &e);
 	CHECK_RS2();
 
-	rs2_delete_device_list(device_list);
+	rsc->config = rs2_create_config(&e);
+	CHECK_RS2();
 
-	U_LOG_D("There are %d connected RealSense devices.", dev_count);
-	if (0 == dev_count) {
-		close_ddev(rs);
-		return 1;
+	// Set the pipeline to start specifically on the realsense device the prober selected
+	rsc->device_idx = device_idx;
+	rsc->device = rs2_create_device(rsc->device_list, rsc->device_idx, &e);
+	CHECK_RS2();
+
+	bool ddev_has_serial = rs2_supports_device_info(rsc->device, RS2_CAMERA_INFO_SERIAL_NUMBER, &e);
+	CHECK_RS2();
+
+	if (ddev_has_serial) {
+
+		const char *ddev_serial = rs2_get_device_info(rsc->device, RS2_CAMERA_INFO_SERIAL_NUMBER, &e);
+		CHECK_RS2();
+
+		rs2_config_enable_device(rsc->config, ddev_serial, &e);
+		CHECK_RS2();
+
+	} else {
+		U_LOG_W("Unexpected, the realsense device in use does not provide a serial number.");
 	}
 
-	rs->pipe = rs2_create_pipeline(rs->ctx, &e);
-	CHECK_RS2();
+	rs2_delete_device(rsc->device);
 
-	rs->config = rs2_create_config(&e);
-	CHECK_RS2();
-
-	rs2_config_enable_stream(rs->config,      //
+	rs2_config_enable_stream(rsc->config,     //
 	                         RS2_STREAM_POSE, // Type
 	                         0,               // Index
 	                         0,               // Width
@@ -175,13 +170,13 @@ create_ddev(struct rs_ddev *rs)
 	                         &e);
 	CHECK_RS2();
 
-	rs2_pipeline_profile *prof = rs2_config_resolve(rs->config, rs->pipe, &e);
+	rsc->profile = rs2_config_resolve(rsc->config, rsc->pipeline, &e);
 	CHECK_RS2();
 
-	rs2_device *cameras = rs2_pipeline_profile_get_device(prof, &e);
+	rsc->device = rs2_pipeline_profile_get_device(rsc->profile, &e);
 	CHECK_RS2();
 
-	rs2_sensor_list *sensors = rs2_query_sensors(cameras, &e);
+	rs2_sensor_list *sensors = rs2_query_sensors(rsc->device, &e);
 	CHECK_RS2();
 
 	//! @todo 0 index hardcoded, check device with RS2_EXTENSION_POSE_SENSOR or similar instead
@@ -203,7 +198,7 @@ create_ddev(struct rs_ddev *rs)
 		CHECK_RS2();
 	}
 
-	rs->profile = rs2_pipeline_start_with_config(rs->pipe, rs->config, &e);
+	rsc->profile = rs2_pipeline_start_with_config(rsc->pipeline, rsc->config, &e);
 	CHECK_RS2();
 
 	rs2_delete_sensor(sensor);
@@ -297,7 +292,7 @@ update(struct rs_ddev *rs)
 	rs2_frame *frames;
 	rs2_error *e = NULL;
 
-	frames = rs2_pipeline_wait_for_frames(rs->pipe, RS2_DEFAULT_TIMEOUT, &e);
+	frames = rs2_pipeline_wait_for_frames(rs->rsc.pipeline, RS2_DEFAULT_TIMEOUT, &e);
 	if (check_error(rs, e) != 0) {
 		return 1;
 	}
@@ -446,7 +441,7 @@ rs_ddev_destroy(struct xrt_device *xdev)
  */
 
 struct xrt_device *
-rs_ddev_create(void)
+rs_ddev_create(int device_idx)
 {
 	struct rs_ddev *rs = U_DEVICE_ALLOCATE(struct rs_ddev, U_DEVICE_ALLOC_TRACKING_NONE, 1, 0);
 
@@ -491,7 +486,7 @@ rs_ddev_create(void)
 		return NULL;
 	}
 
-	ret = create_ddev(rs);
+	ret = create_ddev(rs, device_idx);
 	if (ret != 0) {
 		rs_ddev_destroy(&rs->base);
 		return NULL;

src/xrt/drivers/realsense/rs_driver.h

@@ -0,0 +1,86 @@
+// Copyright 2021, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief  Internal header for the RealSense driver.
+ * @author Mateo de Mayo <mateo.demayo@collabora.com>
+ * @ingroup drv_rs
+ */
+#pragma once
+
+#include "xrt/xrt_prober.h"
+
+#include "rs_interface.h"
+
+#include <librealsense2/rs.h>
+#include <librealsense2/h/rs_pipeline.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/*!
+ * @addtogroup drv_rs
+ * @{
+ */
+
+//! Container to store and manage useful objects from the RealSense API
+struct rs_container
+{
+	rs2_error *error_status;
+
+	// Used by prober and devices
+	rs2_context *context;         //!< RealSense API context
+	rs2_device_list *device_list; //!< List of connected RealSense devices
+	int device_count;             //!< Length of device_list
+
+	// Used by devices
+	int device_idx;                //!< `device` index in `device_list`
+	rs2_device *device;            //!< Main device
+	rs2_pipeline *pipeline;        //!< RealSense running pipeline
+	rs2_config *config;            //!< Pipeline streaming configuration
+	rs2_pipeline_profile *profile; //!< Pipeline profile
+};
+
+//! Cleans up an @ref rs_container and resets its fields to NULL;
+static void
+rs_container_cleanup(struct rs_container *rsc)
+{
+	// A note about what is and what is not being deleted:
+	// In its documentation, the RealSense API specifies which calls require the
+	// caller to delete the returned object afterwards. By looking at the code of
+	// the API it seems that when that is not explicitly pointed out in the
+	// interface documentation, you should *not* delete the returned object.
+
+	// clang-format off
+	if (rsc->profile) rs2_delete_pipeline_profile(rsc->profile);
+	if (rsc->config) rs2_delete_config(rsc->config);
+	if (rsc->pipeline) rs2_delete_pipeline(rsc->pipeline);
+	if (rsc->device) rs2_delete_device(rsc->device);
+	if (rsc->device_list) rs2_delete_device_list(rsc->device_list);
+	if (rsc->context) rs2_delete_context(rsc->context);
+	if (rsc->error_status) rs2_free_error(rsc->error_status);
+	// clang-format on
+
+	rsc->profile = NULL;
+	rsc->config = NULL;
+	rsc->pipeline = NULL;
+	rsc->device = NULL;
+	rsc->device_idx = -1;
+	rsc->device_count = 0;
+	rsc->device_list = NULL;
+	rsc->context = NULL;
+	rsc->error_status = NULL;
+}
+
+//! Create a RealSense device tracked with device-SLAM (T26x).
+struct xrt_device *
+rs_ddev_create(int device_idx);
+
+/*!
+ * @}
+ */
+
+#ifdef __cplusplus
+}
+#endif

src/xrt/drivers/realsense/rs_interface.h

@@ -13,7 +13,6 @@
 extern "C" {
 #endif
 
-
 /*!
  * @defgroup drv_rs Intel RealSense driver
  * @ingroup drv
@@ -21,13 +20,10 @@ extern "C" {
  * @brief Driver for the SLAM-capable Intel Realsense devices.
  */
 
-/*!
- * Create a RealSense device tracked with device-SLAM (T26x).
- *
- * @ingroup drv_rs
- */
-struct xrt_device *
-rs_ddev_create(void);
+#define RS_TRACKING_DISABLED -1
+#define RS_TRACKING_UNSPECIFIED 0
+#define RS_TRACKING_DEVICE_SLAM 1
+#define RS_TRACKING_HOST_SLAM 2
 
 /*!
  * Create a auto prober for rs devices.

src/xrt/drivers/realsense/rs_prober.c

@@ -11,12 +11,45 @@
 #include <stdio.h>
 #include <stdlib.h>
 
+#include "xrt/xrt_config_have.h"
 #include "xrt/xrt_prober.h"
 
 #include "util/u_misc.h"
 #include "util/u_debug.h"
 
-#include "rs_interface.h"
+#include "rs_driver.h"
+
+#include <librealsense2/rs.h>
+
+#define INFO(...) U_LOG(U_LOGGING_INFO, __VA_ARGS__);
+#define WARN(...) U_LOG(U_LOGGING_WARN, __VA_ARGS__);
+#define ERROR(...) U_LOG(U_LOGGING_ERROR, __VA_ARGS__);
+
+//! Utility for realsense API calls that can produce errors
+#define DO(call, ...)                                                                                                  \
+	call(__VA_ARGS__, &e);                                                                                         \
+	check_error(e, __FILE__, __LINE__);
+
+/*!
+ * @brief Specifies which realsense tracking to use
+ * -1 for DISABLED, will not create any RealSense device
+ * 0 for UNSPECIFIED, will decide based on what's available
+ * 1 for DEVICE_SLAM, will only try to use in-device SLAM tracking
+ * 2 for HOST_SLAM, will only try to use external SLAM tracking
+ */
+DEBUG_GET_ONCE_NUM_OPTION(rs_tracking, "RS_TRACKING", RS_TRACKING_UNSPECIFIED)
+
+static bool
+check_error(rs2_error *e, const char *file, int line)
+{
+	if (e == NULL) {
+		return false; // No errors
+	}
+
+	ERROR("rs_error was raised when calling %s(%s):", rs2_get_failed_function(e), rs2_get_failed_args(e));
+	ERROR("%s:%d: %s", file, line, rs2_get_error_message(e));
+	exit(EXIT_FAILURE);
+}
 
 /*!
  * @implements xrt_auto_prober
@@ -42,6 +75,151 @@ rs_prober_destroy(struct xrt_auto_prober *p)
 	free(dp);
 }
 
+/*!
+ * @brief Explores a realsense device to see what SLAM capabilities it supports
+ *
+ * @param[out] out_hslam Whether it supports host-SLAM tracking (Has camera-imu streams)
+ * @param[out] out_dslam Whether it supports device-SLAM tracking (T26x)
+ */
+static void
+check_slam_capabilities(rs2_device_list *device_list, int dev_idx, bool *out_hslam, bool *out_dslam)
+{
+	//! @todo Consider adding the sensors list to the rs_container
+	bool video_sensor_found = false;
+	bool imu_sensor_found = false;
+	bool pose_sensor_found = false;
+
+	rs2_error *e = NULL;
+	rs2_device *device = DO(rs2_create_device, device_list, dev_idx);
+	rs2_sensor_list *sensors = DO(rs2_query_sensors, device);
+	int sensors_count = DO(rs2_get_sensors_count, sensors);
+	for (int i = 0; i < sensors_count; i++) {
+		rs2_sensor *sensor = DO(rs2_create_sensor, sensors, i);
+		video_sensor_found |= DO(rs2_is_sensor_extendable_to, sensor, RS2_EXTENSION_VIDEO);
+		imu_sensor_found |= DO(rs2_is_sensor_extendable_to, sensor, RS2_EXTENSION_MOTION_SENSOR);
+		pose_sensor_found |= DO(rs2_is_sensor_extendable_to, sensor, RS2_EXTENSION_POSE_SENSOR);
+		rs2_delete_sensor(sensor);
+	}
+
+	rs2_delete_sensor_list(sensors);
+	rs2_delete_device(device);
+
+	*out_hslam = video_sensor_found && imu_sensor_found;
+	*out_dslam = pose_sensor_found;
+}
+
+static bool
+supports_host_slam(rs2_device_list *device_list, int index)
+{
+	bool supported, _;
+	check_slam_capabilities(device_list, index, &supported, &_);
+	return supported;
+}
+
+static bool
+supports_device_slam(rs2_device_list *device_list, int index)
+{
+	bool supported, _;
+	check_slam_capabilities(device_list, index, &_, &supported);
+	return supported;
+}
+
+//! @return index of the first device in device_list that has the requested
+//! capability or -1 if none.
+static int
+find_capable_device(int capability, rs2_device_list *device_list)
+{
+	rs2_error *e = NULL;
+	int device_count = DO(rs2_get_device_count, device_list);
+
+	// Determine predicate to check if a device supports the capability
+	bool (*supports_capability)(rs2_device_list *, int);
+	if (capability == RS_TRACKING_DEVICE_SLAM) {
+		supports_capability = supports_device_slam;
+	} else if (capability == RS_TRACKING_HOST_SLAM) {
+		supports_capability = supports_host_slam;
+	} else {
+		ERROR("Invalid capability=%d requested", capability);
+		return false;
+	}
+
+	// Find the first device that support the capability
+	int cdev_idx = -1; // cdev means capable device
+	for (int i = 0; i < device_count; i++) {
+		if (supports_capability(device_list, i)) {
+			cdev_idx = i;
+			break;
+		}
+	}
+
+	return cdev_idx;
+}
+
+//! Implements the conditional flow to decide on how to pick which tracking to use
+static struct xrt_device *
+create_tracked_rs_device(struct xrt_prober *xp)
+{
+	rs2_error *e = NULL;
+	struct rs_container rsc = {0};
+	int expected_tracking = debug_get_num_option_rs_tracking();
+#ifdef XRT_HAVE_SLAM
+	bool external_slam_supported = true;
+#else
+	bool external_slam_supported = false;
+#endif
+
+	rsc.context = DO(rs2_create_context, RS2_API_VERSION);
+	rsc.device_list = DO(rs2_query_devices, rsc.context);
+	rsc.device_count = DO(rs2_get_device_count, rsc.device_list);
+
+	if (rsc.device_count == 0) {
+		if (expected_tracking != RS_TRACKING_UNSPECIFIED) {
+			WARN("RS_TRACKING=%d provided but no RealSense devices found", expected_tracking);
+		}
+		rs_container_cleanup(&rsc);
+		return 0;
+	}
+
+	int ddev_idx = find_capable_device(RS_TRACKING_DEVICE_SLAM, rsc.device_list);
+	bool has_ddev = ddev_idx != -1;
+
+	int hdev_idx = find_capable_device(RS_TRACKING_HOST_SLAM, rsc.device_list);
+	bool has_hdev = hdev_idx != -1;
+
+	rs_container_cleanup(&rsc); // We got ddev_idx and hdev_idx, release realsense resources
+
+	struct xrt_device *dev = NULL;
+	if (expected_tracking == RS_TRACKING_HOST_SLAM) {
+		if (!external_slam_supported) {
+			ERROR("No external SLAM systems built, unable to produce host SLAM tracking");
+		} else if (has_hdev) {
+			ERROR("Host-SLAM device creation not implemented") //! @todo
+		} else {
+			ERROR("No RealSense devices that support external SLAM tracking were found");
+		}
+	} else if (expected_tracking == RS_TRACKING_DEVICE_SLAM) {
+		if (has_ddev) {
+			dev = rs_ddev_create(ddev_idx);
+		} else {
+			WARN("No RealSense devices that support in-device SLAM tracking were found");
+		}
+	} else if (expected_tracking == RS_TRACKING_UNSPECIFIED) {
+		if (has_ddev) {
+			dev = rs_ddev_create(ddev_idx);
+		} else if (has_hdev && external_slam_supported) {
+			ERROR("Host-SLAM device creation not implemented") //! @todo
+		} else {
+			INFO("No RealSense devices that can be tracked were found");
+		}
+	} else if (expected_tracking == RS_TRACKING_DISABLED) {
+		INFO("RS_TRACKING=%d (DISABLED) so skipping any RealSense device", RS_TRACKING_DISABLED);
+	} else {
+		ERROR("Invalid RS_TRACKING=%d", expected_tracking);
+	}
+
+	return dev;
+}
+
 //! @public @memberof rs_prober
 static int
 rs_prober_autoprobe(struct xrt_auto_prober *xap,
@@ -53,7 +231,7 @@ rs_prober_autoprobe(struct xrt_auto_prober *xap,
 	struct rs_prober *dp = rs_prober(xap);
 	(void)dp;
 
-	struct xrt_device *dev = rs_ddev_create();
+	struct xrt_device *dev = create_tracked_rs_device(xp);
 	if (!dev) {
 		return 0;
 	}