monado/src/xrt/drivers/realsense/rs_prober.c

// Copyright 2021, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
 * @file
 * @brief  Realsense prober code.
 * @author Christoph Haag <christoph.haag@collabora.com>
 * @author Jakob Bornecrantz <jakob@collabora.com>
 * @ingroup drv_realsense
 */

#include <stdio.h>
#include <stdlib.h>

#include "xrt/xrt_config_build.h"
#include "xrt/xrt_config_have.h"
#include "xrt/xrt_prober.h"

#include "util/u_misc.h"
#include "util/u_debug.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
 */
struct rs_prober
{
	struct xrt_auto_prober base;
};

//! @private @memberof rs_prober
static inline struct rs_prober *
rs_prober(struct xrt_auto_prober *p)
{
	return (struct rs_prober *)p;
}

//! @public @memberof rs_prober
static void
rs_prober_destroy(struct xrt_auto_prober *p)
{
	struct rs_prober *dp = rs_prober(p);

	free(dp);
}

/*!
 * @brief Explores a realsense device to see what SLAM capabilities it supports
 *
 * @param device_list List in which the device resides.
 * @param dev_idx Index of the device in @p device_list.
 * @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_FEATURE_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) {
			dev = rs_hdev_create(xp, hdev_idx);
		} 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) {
			dev = rs_hdev_create(xp, hdev_idx);
		} 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,
                    cJSON *attached_data,
                    bool no_hmds,
                    struct xrt_prober *xp,
                    struct xrt_device **out_xdevs)
{
	struct rs_prober *dp = rs_prober(xap);
	(void)dp;

	struct xrt_device *dev = create_tracked_rs_device(xp);
	if (!dev) {
		return 0;
	}

	out_xdevs[0] = dev;
	return 1;
}

struct xrt_auto_prober *
rs_create_auto_prober()
{
	struct rs_prober *dp = U_TYPED_CALLOC(struct rs_prober);
	dp->base.name = "Realsense";
	dp->base.destroy = rs_prober_destroy;
	dp->base.lelo_dallas_autoprobe = rs_prober_autoprobe;

	return &dp->base;
}