d/dai: Add simple DepthAI driver

Co-authored-by: Jakob Bornecrantz <jakob@collabora.com>

CMakeLists.txt

@@ -54,6 +54,7 @@ find_package(OpenCV COMPONENTS core calib3d highgui imgproc imgcodecs features2d
 find_package(Libusb1)
 find_package(JPEG)
 find_package(realsense2 CONFIG)
+find_package(depthai CONFIG)
 find_package(SDL2 CONFIG)
 find_package(ZLIB)
 find_package(cJSON)
@@ -215,6 +216,7 @@ cmake_dependent_option(XRT_BUILD_DRIVER_PSMV "Enable Playstation Move driver" ON
 cmake_dependent_option(XRT_BUILD_DRIVER_HYDRA "Enable Hydra driver" ON "XRT_HAVE_INTERNAL_HID" OFF)
 cmake_dependent_option(XRT_BUILD_DRIVER_NS "Enable North Star driver" ON "XRT_HAVE_INTERNAL_HID" OFF)
 cmake_dependent_option(XRT_BUILD_DRIVER_VF "Build video frame driver (for video file support, uses gstreamer)" ON "XRT_HAVE_GST" OFF)
+cmake_dependent_option(XRT_BUILD_DRIVER_DEPTHAI "DepthAI" ON "depthai_FOUND" OFF)
 
 # This one defaults to off, even if we find the deps.
 cmake_dependent_option(XRT_BUILD_DRIVER_SURVIVE "Enable libsurvive driver" ON "SURVIVE_FOUND" OFF)
@@ -243,6 +245,7 @@ list(APPEND AVAILABLE_DRIVERS
 	"V4L2"
 	"ULV2"
 	"VF"
+	"DEPTHAI"
 	"VIVE"
 	"QWERTY"
 	"WMR"
@@ -384,6 +387,7 @@ message(STATUS "#    DRIVER_RS:           ${XRT_BUILD_DRIVER_RS}")
 message(STATUS "#    DRIVER_REMOTE:       ${XRT_BUILD_DRIVER_REMOTE}")
 message(STATUS "#    DRIVER_SURVIVE:      ${XRT_BUILD_DRIVER_SURVIVE}")
 message(STATUS "#    DRIVER_VF:           ${XRT_BUILD_DRIVER_VF}")
+message(STATUS "#    DRIVER_DEPTHAI:      ${XRT_BUILD_DRIVER_DEPTHAI}")
 message(STATUS "#    DRIVER_VIVE:         ${XRT_BUILD_DRIVER_VIVE}")
 message(STATUS "#    DRIVER_QWERTY:       ${XRT_BUILD_DRIVER_QWERTY}")
 message(STATUS "#    DRIVER_WMR:          ${XRT_BUILD_DRIVER_WMR}")

meson.build

@@ -77,6 +77,7 @@ libbsd   = dependency('libbsd', required: get_option('libbsd'))
 gst      = dependency('gstreamer-1.0', required: false)
 gst_app  = dependency('gstreamer-app-1.0', required: false)
 gst_video= dependency('gstreamer-video-1.0', required: false)
+depthai  = dependency('depthai', method: 'cmake', modules : ['depthai::core', 'depthai::opencv', 'XLink'], required: false)
 
 gst_found = gst.found() and gst_app.found() and gst_video.found()
 
@@ -212,6 +213,12 @@ if gst_found and ('auto' in drivers or 'vf' in drivers)
 	endif
 endif
 
+if depthai.found() and ('auto' in drivers or 'depthai' in drivers)
+	if 'depthai' not in drivers
+		drivers += ['depthai']
+	endif
+endif
+
 if leap.found() and ('auto' in drivers or 'ulv2' in drivers)
        if 'ulv2' not in drivers
                drivers += ['ulv2']

meson_options.txt

@@ -3,7 +3,7 @@
 
 option('drivers',
 	type: 'array',
-	choices: ['auto', 'dummy', 'hdk', 'hydra', 'ns', 'ohmd', 'psmv', 'psvr', 'rs', 'v4l2', 'vf', 'vive', 'wmr', 'survive', 'daydream', 'arduino', 'remote', 'handtracking', 'qwerty', 'ulv2'],
+	choices: ['auto', 'dummy', 'hdk', 'hydra', 'ns', 'ohmd', 'psmv', 'psvr', 'rs', 'v4l2', 'vf', 'depthai', 'vive', 'wmr', 'survive', 'daydream', 'arduino', 'remote', 'handtracking', 'qwerty', 'ulv2'],
 	value: ['auto'],
 	description: 'Set of drivers to build')
 

src/xrt/drivers/CMakeLists.txt

@@ -31,6 +31,25 @@ if(XRT_BUILD_DRIVER_DAYDREAM)
 	list(APPEND ENABLED_DRIVERS daydream)
 endif()
 
+if(XRT_BUILD_DRIVER_DEPTHAI)
+	set(DEPTHAI_SOURCE_FILES
+		depthai/depthai_driver.cpp
+		depthai/depthai_interface.h
+		)
+
+	add_library(drv_depthai STATIC ${DEPTHAI_SOURCE_FILES})
+	target_link_libraries(drv_depthai PRIVATE
+		xrt-interfaces
+		aux_os
+		${OpenCV_LIBRARIES}
+		depthai::opencv
+		depthai::core
+		XLink
+		)
+	target_include_directories(drv_depthai PRIVATE ${OpenCV_INCLUDE_DIRS})
+	list(APPEND ENABLED_DRIVERS depthai)
+endif()
+
 if(XRT_BUILD_DRIVER_DUMMY)
 	set(DUMMY_SOURCE_FILES
 		dummy/dummy_hmd.c

src/xrt/drivers/depthai/depthai_driver.cpp

@@ -0,0 +1,366 @@
+// Copyright 2019-2021, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief  DepthAI frameserver implementation.
+ * @author Moses Turner <moses@collabora.com>
+ * @author Jakob Bornecrantz <jakob@collabora.com>
+ * @ingroup drv_depthai
+ */
+
+#include "os/os_time.h"
+#include "os/os_threading.h"
+
+#include "util/u_var.h"
+#include "util/u_misc.h"
+#include "util/u_debug.h"
+#include "util/u_frame.h"
+#include "util/u_format.h"
+#include "util/u_logging.h"
+
+#include "depthai_interface.h"
+
+#include "depthai/depthai.hpp"
+
+#include <stdio.h>
+#include <assert.h>
+#include <unistd.h>
+#include <pthread.h>
+
+#include <memory>
+#include <sstream>
+
+
+/*
+ *
+ * Printing functions.
+ *
+ */
+
+#define DEPTHAI_TRACE(d, ...) U_LOG_IFL_T(d->ll, __VA_ARGS__)
+#define DEPTHAI_DEBUG(d, ...) U_LOG_IFL_D(d->ll, __VA_ARGS__)
+#define DEPTHAI_INFO(d, ...) U_LOG_IFL_I(d->ll, __VA_ARGS__)
+#define DEPTHAI_WARN(d, ...) U_LOG_IFL_W(d->ll, __VA_ARGS__)
+#define DEPTHAI_ERROR(d, ...) U_LOG_IFL_E(d->ll, __VA_ARGS__)
+
+DEBUG_GET_ONCE_LOG_OPTION(depthai_log, "DEPTHAI_LOG", U_LOGGING_WARN)
+
+
+/*
+ *
+ * Helper frame wrapper code.
+ *
+ */
+
+class depthairameWrapper
+{
+public:
+	struct xrt_frame frame = {};
+
+	std::shared_ptr<dai::ImgFrame> depthai_frame = {};
+};
+
+extern "C" void
+depthai_frame_wrapper_destroy(struct xrt_frame *xf)
+{
+	depthairameWrapper *dfw = (depthairameWrapper *)xf;
+	delete dfw;
+}
+
+
+/*
+ *
+ * DepthAI frameserver.
+ *
+ */
+
+struct depthai_fs
+{
+	struct xrt_fs base;
+	struct xrt_frame_node node;
+	struct os_thread_helper play_thread;
+
+	u_logging_level ll;
+
+	uint32_t width;
+	uint32_t height;
+
+	xrt_frame_sink *sink;
+
+	dai::Device *device;
+	dai::DataOutputQueue *queue;
+};
+
+
+/*
+ *
+ * Internal functions.
+ *
+ */
+
+static void
+depthai_print_connected_cameras(struct depthai_fs *depthai)
+{
+	std::ostringstream oss = {};
+	for (const auto &cam : depthai->device->getConnectedCameras()) {
+		oss << "'" << static_cast<int>(cam) << "' ";
+	}
+	std::string str = oss.str();
+
+	DEPTHAI_DEBUG(depthai, "DepthAI: Connected cameras: %s", str.c_str());
+}
+
+static void
+depthai_do_one_frame(struct depthai_fs *depthai)
+{
+	std::shared_ptr<dai::ImgFrame> imgFrame = depthai->queue->get<dai::ImgFrame>();
+	if (!imgFrame) {
+		std::cout << "Not ImgFrame" << std::endl;
+		return; // Nothing to do.
+	}
+
+	// Get the timestamp.
+	auto duration = imgFrame->getTimestamp().time_since_epoch();
+	auto nano = std::chrono::duration_cast<std::chrono::duration<int64_t, std::nano>>(duration);
+	uint64_t timestamp_ns = nano.count();
+
+	// Create a wrapper that will keep the frame alive as long as the frame was alive.
+	depthairameWrapper *dfw = new depthairameWrapper();
+	dfw->depthai_frame = imgFrame;
+
+	// Fill in all of the data.
+	struct xrt_frame *xf = &dfw->frame;
+	xf->reference.count = 1;
+	xf->destroy = depthai_frame_wrapper_destroy;
+	xf->width = depthai->width;
+	xf->height = depthai->height;
+	xf->format = XRT_FORMAT_R8G8B8;
+	xf->timestamp = timestamp_ns;
+	xf->data = imgFrame->getData().data();
+
+	// Calculate stride and size, assuming tightly packed rows.
+	u_format_size_for_dimensions(xf->format, xf->width, xf->height, &xf->stride, &xf->size);
+
+	// Push the frame to the sink.
+	xrt_sink_push_frame(depthai->sink, xf);
+
+	// If downstream wants to keep the frame they would have referenced it.
+	xrt_frame_reference(&xf, NULL);
+}
+
+static void *
+depthai_mainloop(void *ptr)
+{
+	struct depthai_fs *depthai = (struct depthai_fs *)ptr;
+	DEPTHAI_DEBUG(depthai, "DepthAI: Mainloop called");
+
+	os_thread_helper_lock(&depthai->play_thread);
+	while (os_thread_helper_is_running_locked(&depthai->play_thread)) {
+		os_thread_helper_unlock(&depthai->play_thread);
+
+		depthai_do_one_frame(depthai);
+
+		// Need to lock the thread when we go back to the while condition.
+		os_thread_helper_lock(&depthai->play_thread);
+	}
+	os_thread_helper_unlock(&depthai->play_thread);
+
+	DEPTHAI_DEBUG(depthai, "DepthAI: Mainloop exiting");
+	return nullptr;
+}
+
+static bool
+depthai_destroy(struct depthai_fs *depthai)
+{
+	DEPTHAI_DEBUG(depthai, "DepthAI: Frameserver destroy called");
+
+	os_thread_helper_destroy(&depthai->play_thread);
+
+	delete depthai->device;
+
+	free(depthai);
+
+	return true;
+}
+
+
+/*
+ *
+ * Frame server functions.
+ *
+ */
+
+/*!
+ * Cast to derived type.
+ */
+static inline struct depthai_fs *
+depthai_fs(struct xrt_fs *xfs)
+{
+	return (struct depthai_fs *)xfs;
+}
+
+static bool
+depthai_fs_enumerate_modes(struct xrt_fs *xfs, struct xrt_fs_mode **out_modes, uint32_t *out_count)
+{
+	struct depthai_fs *depthai = depthai_fs(xfs);
+	DEPTHAI_DEBUG(depthai, "DepthAI: Enumerate modes called");
+
+	struct xrt_fs_mode *modes = U_TYPED_ARRAY_CALLOC(struct xrt_fs_mode, 1);
+	if (modes == NULL) {
+		return false;
+	}
+
+	modes[0].width = depthai->width;
+	modes[0].height = depthai->height;
+	modes[0].format = XRT_FORMAT_R8G8B8;
+	modes[0].stereo_format = XRT_STEREO_FORMAT_NONE;
+
+	*out_modes = modes;
+	*out_count = 1;
+
+	return true;
+}
+
+static bool
+depthai_fs_configure_capture(struct xrt_fs *xfs, struct xrt_fs_capture_parameters *cp)
+{
+	struct depthai_fs *depthai = depthai_fs(xfs);
+	DEPTHAI_DEBUG(depthai, "DepthAI: Configure capture called");
+
+	// Noop
+	return false;
+}
+
+static bool
+depthai_fs_stream_start(struct xrt_fs *xfs,
+                        struct xrt_frame_sink *xs,
+                        enum xrt_fs_capture_type capture_type,
+                        uint32_t descriptor_index)
+{
+	struct depthai_fs *depthai = depthai_fs(xfs);
+	DEPTHAI_DEBUG(depthai, "DepthAI: Stream start called");
+
+	assert(descriptor_index == 0);
+	(void)capture_type; // Don't care about this one just yet.
+
+	depthai->sink = xs;
+
+	os_thread_helper_start(&depthai->play_thread, depthai_mainloop, depthai);
+
+	return true;
+}
+
+static bool
+depthai_fs_stream_stop(struct xrt_fs *xfs)
+{
+	struct depthai_fs *depthai = depthai_fs(xfs);
+	DEPTHAI_DEBUG(depthai, "DepthAI: Stream stop called");
+
+	// This call fully stops the thread.
+	os_thread_helper_stop(&depthai->play_thread);
+
+	return true;
+}
+
+static bool
+depthai_fs_is_running(struct xrt_fs *xfs)
+{
+	struct depthai_fs *depthai = depthai_fs(xfs);
+
+	os_thread_helper_lock(&depthai->play_thread);
+	bool running = os_thread_helper_is_running_locked(&depthai->play_thread);
+	os_thread_helper_unlock(&depthai->play_thread);
+
+	return running;
+}
+
+
+/*
+ *
+ * Node functions.
+ *
+ */
+
+static void
+depthai_fs_node_break_apart(struct xrt_frame_node *node)
+{
+	struct depthai_fs *depthai = container_of(node, struct depthai_fs, node);
+	DEPTHAI_DEBUG(depthai, "DepthAI: Node break apart called");
+
+	depthai_fs_stream_stop(&depthai->base);
+}
+
+static void
+depthai_fs_node_destroy(struct xrt_frame_node *node)
+{
+	struct depthai_fs *depthai = container_of(node, struct depthai_fs, node);
+	DEPTHAI_DEBUG(depthai, "DepthAI: Node destroy called");
+
+	// Safe to call, break apart have already stopped the stream.
+	depthai_destroy(depthai);
+}
+
+
+/*
+ *
+ * 'Exported' functions.
+ *
+ */
+
+extern "C" struct xrt_fs *
+depthai_fs_single_rgb(struct xrt_frame_context *xfctx)
+{
+	// Try to create a device and see if that fail first.
+	dai::Device *d;
+	try {
+		d = new dai::Device();
+	} catch (std::exception &e) {
+		std::string what = e.what();
+		U_LOG_E("DepthAI error: %s", what.c_str());
+		return nullptr;
+	}
+
+	struct depthai_fs *depthai = U_TYPED_CALLOC(struct depthai_fs);
+	depthai->base.enumerate_modes = depthai_fs_enumerate_modes;
+	depthai->base.configure_capture = depthai_fs_configure_capture;
+	depthai->base.stream_start = depthai_fs_stream_start;
+	depthai->base.stream_stop = depthai_fs_stream_stop;
+	depthai->base.is_running = depthai_fs_is_running;
+	depthai->node.break_apart = depthai_fs_node_break_apart;
+	depthai->node.destroy = depthai_fs_node_destroy;
+	depthai->ll = debug_get_log_option_depthai_log();
+	depthai->width = 1280;
+	depthai->height = 800; // Constant for now
+	depthai->device = d;
+
+	// Some debug printing.
+	depthai_print_connected_cameras(depthai);
+
+	// Make sure that the thread helper is initialised.
+	os_thread_helper_init(&depthai->play_thread);
+
+	dai::Pipeline p = {};
+
+	auto colorCam = p.create<dai::node::ColorCamera>();
+	auto xlinkOut = p.create<dai::node::XLinkOut>();
+	xlinkOut->setStreamName("preview");
+	colorCam->setPreviewSize(depthai->width, depthai->height);
+	colorCam->setResolution(dai::ColorCameraProperties::SensorResolution::THE_800_P);
+	colorCam->setInterleaved(true);
+	colorCam->setImageOrientation(dai::CameraImageOrientation::ROTATE_180_DEG);
+	colorCam->setFps(60);
+	colorCam->setColorOrder(dai::ColorCameraProperties::ColorOrder::RGB);
+
+	// Link plugins CAM -> XLINK
+	colorCam->preview.link(xlinkOut->input);
+
+	// Start the pipeline
+	d->startPipeline(p);
+	depthai->queue = d->getOutputQueue("preview", 1, false).get(); // out of shared pointer
+
+	xrt_frame_context_add(xfctx, &depthai->node);
+
+	DEPTHAI_DEBUG(depthai, "DepthAI: Created");
+
+	return &depthai->base;
+}

src/xrt/drivers/depthai/depthai_interface.h

@@ -0,0 +1,38 @@
+// Copyright 2019-2021, Collabora, Ltd.
+// SPDX-License-Identifier: BSL-1.0
+/*!
+ * @file
+ * @brief  Interface header for DepthAI camera.
+ * @author Moses Turner <moses@collabora.com>
+ * @author Jakob Bornecrantz <jakob@collabora.com>
+ * @ingroup drv_depthai
+ */
+
+#pragma once
+
+#include "xrt/xrt_frameserver.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/*!
+ * @defgroup drv_depthai DepthAI frameserver driver
+ * @ingroup drv
+ *
+ * @brief Frameserver for the DepthAI camera module.
+ */
+
+/*!
+ * Create a DepthAI frameserver using a single RGB camera.
+ *
+ * @ingroup drv_depthai
+ */
+struct xrt_fs *
+depthai_fs_single_rgb(struct xrt_frame_context *xfctx);
+
+
+#ifdef __cplusplus
+}
+#endif

src/xrt/drivers/meson.build

@@ -3,6 +3,21 @@
 
 drv_include = include_directories('.')
 
+lib_drv_depthai = static_library(
+	'drv_depthai',
+	files(
+		'depthai/depthai_driver.cpp',
+	),
+	include_directories: [xrt_include],
+	dependencies: [aux, opencv, depthai],
+	build_by_default: 'depthai' in drivers,
+)
+
+drv_depthai = declare_dependency(
+	include_directories: drv_include,
+	link_with: lib_drv_depthai
+)
+
 lib_drv_dummy = static_library(
 	'drv_dummy',
 	files(