diff --git a/orx-kinect-common/src/main/kotlin/Kinect.kt b/orx-kinect-common/src/main/kotlin/Kinect.kt index 22a63309..22e0073a 100644 --- a/orx-kinect-common/src/main/kotlin/Kinect.kt +++ b/orx-kinect-common/src/main/kotlin/Kinect.kt @@ -1,64 +1,44 @@ package org.openrndr.extra.kinect import org.openrndr.Extension -import org.openrndr.Program import org.openrndr.draw.* -import org.openrndr.math.Vector2 -import org.openrndr.resourceUrl import java.lang.RuntimeException -import java.nio.ByteBuffer -import java.util.concurrent.atomic.AtomicReference -import kotlin.concurrent.thread -interface Kinects { +/** + * Represents all the accessible kinects handled by a specific driver (V1, V2). + * + * @param data needed to make low level kinect support calls. + */ +interface Kinects { fun countDevices(): Int /** * Starts kinect device of a given number. * - * @throws KinectException if device of such a number does not exist. + * @throws KinectException if device of such a number does not exist, + * better to count them first. + * @see countDevices */ - fun startDevice(num: Int = 0): KinectDevice + fun startDevice(num: Int = 0): KinectDevice + + /** + * Executes low level Kinect commands in the kinect thread. + */ + fun execute(commands: (CTX) -> Any) : Any } -class DefaultKinects(private val manager: KinectsManager) : Kinects { - init { - manager.init() - // as we don't have explicit shutdown mechanism in OPENRNDR - // we need to install a shutdown hook for now - Runtime.getRuntime().addShutdownHook( - thread( - name = "${manager.javaClass.simpleName}-closer", - start = false, - isDaemon = false - ) { - manager.shutdown() - } - ) - } - override fun countDevices(): Int { - return manager.countDevices() - } - override fun startDevice(num: Int): KinectDevice { - return manager.startDevice(num) - } -} +/** + * Represents specific device. + * + * @param data needed to make low level kinect support calls. + */ +interface KinectDevice : Extension { + val depthCamera: KinectDepthCamera -interface KinectsManager { - fun init() - fun countDevices(): Int - fun startDevice(num: Int): KinectDevice - fun shutdown() -} - -abstract class KinectDevice : Extension { - override var enabled: Boolean = true - abstract val depthCamera: KinectDepthCamera - override fun beforeDraw(drawer: Drawer, program: Program) { - if (depthCamera.enabled) { - depthCamera.update() - } - } + /** + * Executes low level Kinect commands in the kinect thread in the context of this device. + */ + fun execute(commands: (CTX) -> Any): Any } interface KinectCamera { @@ -68,58 +48,10 @@ interface KinectCamera { var mirror: Boolean val currentFrame: ColorBuffer fun getLatestFrame(): ColorBuffer? - fun update() } interface KinectDepthCamera : KinectCamera { - val depthScale: Double -} - -abstract class AbstractKinectDepthCamera( - final override val width: Int, - final override val height: Int, - final override val depthScale: Double -) : KinectDepthCamera { - protected val byteBufferRef: AtomicReference = AtomicReference() - private val rawBuffer: ColorBuffer = colorBuffer( - width, - height, - format = ColorFormat.R, - type = ColorType.UINT16 // it would be perfect if we could use isampler in the shader - ) - override val currentFrame: ColorBuffer = colorBuffer( - width, - height, - format = ColorFormat.R, - type = ColorType.FLOAT16 // in the future we might want to choose the precision here - ) - private val depthMapper = KinectRawDataToDepthMapper() - init { - depthMapper.depthScale = depthScale - depthMapper.mirror = false - depthMapper.resolution = Vector2(width.toDouble(), height.toDouble()) - } - private val newFrameRef = AtomicReference() - override fun getLatestFrame(): ColorBuffer? { - return newFrameRef.getAndSet(null) - } - override fun update() { - byteBufferRef.getAndSet(null)?.let { bytes -> - rawBuffer.write(bytes) - depthMapper.apply(rawBuffer, currentFrame) - newFrameRef.set(currentFrame) - } - } - override var mirror: Boolean - get() = depthMapper.mirror - set(value) { depthMapper.mirror = value } -} - -class KinectRawDataToDepthMapper : - Filter(filterShaderFromUrl(resourceUrl("kinect-raw-to-depth.frag", Kinects::class.java))) { - var depthScale: Double by parameters - var mirror: Boolean by parameters - var resolution: Vector2 by parameters + /* no special attributes at the moment */ } class KinectException(msg: String) : RuntimeException(msg) diff --git a/orx-kinect-common/src/main/kotlin/KinectImpl.kt b/orx-kinect-common/src/main/kotlin/KinectImpl.kt new file mode 100644 index 00000000..85d3e12e --- /dev/null +++ b/orx-kinect-common/src/main/kotlin/KinectImpl.kt @@ -0,0 +1,139 @@ +package org.openrndr.extra.kinect + +import org.openrndr.Program +import org.openrndr.draw.* +import org.openrndr.math.Vector2 +import org.openrndr.resourceUrl +import java.nio.ByteBuffer +import java.util.concurrent.atomic.AtomicReference +import java.util.function.Supplier +import kotlin.concurrent.thread + + +class DefaultKinects(private val manager: KinectsManager) : Kinects { + init { + manager.initialize() + // as we don't have explicit shutdown mechanism in OPENRNDR + // we need to install a shutdown hook for now + Runtime.getRuntime().addShutdownHook( + thread( + name = "${manager.javaClass.simpleName}-closer", + start = false, + isDaemon = false + ) { + manager.shutdown() + } + ) + } + + override fun countDevices(): Int { + return manager.countDevices() + } + + override fun startDevice(num: Int): KinectDevice { + return manager.startDevice(num) + } + + override fun execute(commands: (CTX) -> Any): Any { + return manager.execute(commands) + } + +} + +interface KinectsManager { + fun initialize() + fun countDevices(): Int + fun startDevice(num: Int): KinectDevice + fun execute(commands: (CTX) -> Any): Any + fun shutdown() +} + +interface KinectFeatureEnabler { + var enabled: Boolean +} + +interface KinectCommandsExecutor { + fun execute(commands: (CTX) -> Any): Any +} + +class DefaultKinectDevice( + override val depthCamera: DefaultKinectDepthCamera, + private val commandsExecutor: KinectCommandsExecutor +) : KinectDevice { + override var enabled: Boolean = true + override fun beforeDraw(drawer: Drawer, program: Program) { + depthCamera.update() + } + override fun execute(commands: (CTX) -> Any): Any { + return commandsExecutor.execute(commands) + } +} + +class DefaultKinectDepthCamera( + override val width: Int, + override val height: Int, + depthScale: Double, + private val enabler: KinectFeatureEnabler, + private val bytesSupplier: Supplier +) : + KinectDepthCamera, UpdatableKinectCamera { + + override var enabled: Boolean + get() = enabler.enabled + set(value) { + enabler.enabled = value + } + + private val rawBuffer: ColorBuffer = colorBuffer( + width, + height, + format = ColorFormat.R, + type = ColorType.UINT16 // it would be perfect if we could use isampler in the shader + ) + override val currentFrame: ColorBuffer = colorBuffer( + width, + height, + format = ColorFormat.R, + type = ColorType.FLOAT16 // in the future we might want to choose the precision here + ) + + private val depthMapper = KinectRawDataToDepthMapper() + + init { + depthMapper.depthScale = depthScale + depthMapper.mirror = false + depthMapper.resolution = Vector2(width.toDouble(), height.toDouble()) + } + + private val newFrameRef = AtomicReference() + + override fun getLatestFrame(): ColorBuffer? { + return newFrameRef.getAndSet(null) + } + + override fun update() { + if (enabled) { + bytesSupplier.get()?.let { bytes -> + rawBuffer.write(bytes) + depthMapper.apply(rawBuffer, currentFrame) + newFrameRef.set(currentFrame) + } + } + } + + override var mirror: Boolean + get() = depthMapper.mirror + set(value) { depthMapper.mirror = value } + +} + +private class KinectRawDataToDepthMapper : + Filter(filterShaderFromUrl(resourceUrl("kinect-raw-to-depth.frag", Kinects::class.java))) { + var depthScale: Double by parameters + var mirror: Boolean by parameters + var resolution: Vector2 by parameters +} + +private interface UpdatableKinectCamera { + fun update() +} diff --git a/orx-kinect-v1/src/main/kotlin/KinectV1.kt b/orx-kinect-v1/src/main/kotlin/KinectV1.kt index 5d2ad830..b7a28c3a 100644 --- a/orx-kinect-v1/src/main/kotlin/KinectV1.kt +++ b/orx-kinect-v1/src/main/kotlin/KinectV1.kt @@ -1,22 +1,42 @@ -package org.openrndr.extra.kinect +package org.openrndr.extra.kinect.v1 import mu.KotlinLogging import org.bytedeco.javacpp.Pointer -import org.bytedeco.libfreenect.freenect_context -import org.bytedeco.libfreenect.freenect_depth_cb -import org.bytedeco.libfreenect.freenect_device -import org.bytedeco.libfreenect.freenect_usb_context +import org.bytedeco.libfreenect.* import org.bytedeco.libfreenect.global.freenect.* +import org.bytedeco.libfreenect.presets.freenect +import org.openrndr.extra.kinect.* import java.nio.ByteBuffer import java.nio.ByteOrder import java.util.* -import kotlin.concurrent.thread +import java.util.concurrent.* +import java.util.concurrent.atomic.AtomicBoolean +import java.util.concurrent.atomic.AtomicReference +import java.util.function.Supplier -fun getKinectsV1() : Kinects { - return DefaultKinects(KinectsV1Manager()) +/** + * Returns support for kinect version 1. + * + * @param depthCameraInitializationDelay defaults to 100 ms. Delay seems to be + * necessary due to either my misunderstanding or some weird freenect bug. + * Without the delay between starting depth camera and registering + * depth callback, no frames are transferred at all. However this + * problem happens only on the first try with freshly connected + * kinect. Subsequent runs of the same program don't require + * this delay at all. + */ +fun getKinectsV1(depthCameraInitializationDelay: Long = 100) : Kinects { + return DefaultKinects(KinectsV1Manager(depthCameraInitializationDelay)) } -class KinectsV1Manager : KinectsManager { +/** Provides low level freenect context for calling native freenect methods. */ +class Freenect( + val fnCtx: freenect_context, + val fnUsbCtx: freenect_usb_context, + val fnDev: freenect_device? = null // only available for device level commands +) + +private class KinectsV1Manager(val depthCameraInitializationDelay: Long) : KinectsManager { private val logger = KotlinLogging.logger {} @@ -24,17 +44,109 @@ class KinectsV1Manager : KinectsManager { private val fnUsbCtx = freenect_usb_context() - private var running: Boolean = true + private val ctx = Freenect(fnCtx, fnUsbCtx) - private val devices: LinkedList = LinkedList() + private var running = true - private val poller: Thread = thread(name = "Kinect1-poll", start = false, isDaemon = true) { - while (running && freenect_process_events(fnCtx) >= 0) {} + private val devices: LinkedList = LinkedList() + + private val timeout = freenect.timeval() + init { timeout.tv_sec(1) } + + private val executor = Executors.newSingleThreadExecutor{ + runnable -> Thread(runnable, "Kinect1-runner") } - inner class KinectV1Device(private val num: Int) : KinectDevice() { - override val depthCamera = KinectV1DepthCamera() + private inner class KinectV1CommandsExecutor(val context: Freenect) : KinectCommandsExecutor { + override fun execute(commands: (Freenect) -> Any): Any { + return executor.submit(Callable { + logger.debug { "Executing native freenect commands" } + commands(context) + }).get() + } + } + + private val commandsExecutor = KinectV1CommandsExecutor(ctx) + + override fun initialize() { + logger.info("Initializing Kinect1 support, set log level to TRACE to see received frames") + executor.execute { + logger.debug("Initializing freenect") + verify(freenect_init(fnCtx, fnUsbCtx)) + freenect_set_log_level(fnCtx, FREENECT_LOG_INFO) + freenect_select_subdevices(fnCtx, FREENECT_DEVICE_CAMERA) + val num = verify(freenect_num_devices(fnCtx)) + if (num == 0) { + logger.warn { "Could not find any Kinect1 device, calling startDevice() will throw exception" } + } + } + executor.execute(object : Runnable { + override fun run() { + if (!running) { return } + val ret = freenect_process_events_timeout(fnCtx, timeout) + if (ret != 0) { + logger.error { "freenect_process_events_timeout returned non-zero value: $ret" } + } + executor.execute(this) // loop + } + }) + } + + override fun countDevices(): Int { + return executor.submit( + Callable { verify(freenect_num_devices(fnCtx)) } + ).get() + } + + // FIXME we should prevent from starting the same device multiple times + override fun startDevice(num: Int): KinectDevice { + val count = countDevices() + if (num >= count) { + throw KinectException( + "Trying to start non-existent Kinect1 device, device count: $count, num: $num" + ) + } + val device = executor.submit( + Callable { + val device = FreenectDevice(num) + devices.add(device) + device + } + ).get() + return DefaultKinectDevice( + DefaultKinectDepthCamera( + device.depthCamera.width, + device.depthCamera.height, + 32.0, + device.depthCamera.enabler, + device.depthCamera.bytesSupplier + ), + KinectV1CommandsExecutor(device.devCtx) + ) + } + + override fun execute(commands: (Freenect) -> Any): Any { + return commandsExecutor.execute(commands) + } + + override fun shutdown() { + logger.info("Shutting down Kinect1 support") + executor.submit { running = false }.get() + executor.submit { + if (!fnCtx.isNull) { + devices.forEach { device -> device.shutdown() } + devices.clear() + } + }.get() // wait to finish + executor.shutdown() + executor.awaitTermination(1100, TimeUnit.MILLISECONDS) + // value slightly higher than 1sec polling timeout, just in case + } + + private inner class FreenectDevice(private val num: Int) { + val depthCamera = FreenectDepthCamera() val fnDev = freenect_device() + val devCtx = Freenect(fnCtx, fnUsbCtx, fnDev) init { logger.info { "Opening Kinect1 device num: $num" } verify(freenect_open_device(fnCtx, fnDev, num)) @@ -46,40 +158,62 @@ class KinectsV1Manager : KinectsManager { verifyOnShutdown(freenect_close_device(fnDev)) } } - inner class KinectV1DepthCamera : AbstractKinectDepthCamera( - width = 640, - height = 480, - depthScale = 32.0 - ) { - private val logger = KotlinLogging.logger {} + inner class FreenectDepthCamera { + val width: Int = 640 + val height: Int = 480 private val bytes = ByteBuffer.allocateDirect(width * height * 2) + private val currentBytesRef = AtomicReference() + init { bytes.order(ByteOrder.nativeOrder()) } private val freenectDepthCb = object : freenect_depth_cb() { override fun call(dev: freenect_device?, depth: Pointer?, timestamp: Int) { logger.trace { "depth frame received for Kinect1 device: $num, at: $timestamp" } - byteBufferRef.set(bytes) + currentBytesRef.set(bytes) } } - init { - bytes.order(ByteOrder.nativeOrder()) - } - override var enabled: Boolean = false - set(value) { - field = if (value) { - start() - true - } else { - stop() - false + val bytesSupplier = Supplier { currentBytesRef.getAndSet(null) } + val enabler = object : KinectFeatureEnabler { + private val atomicEnabled = AtomicBoolean(false) + private val inProgress = AtomicBoolean(false) + override var enabled + get() = atomicEnabled.get() + set(value) { + if (atomicEnabled.get() == value) { + logger.warn { "Current state requested - doing nothing, Kinect1 device: $num, enabled=$value" } + } + if (!inProgress.getAndSet(true)) { + if (value) { + executor.execute { + try { + start() + } finally { + inProgress.set(false) + } + } + atomicEnabled.set(true) + } else { + executor.execute { + try { + stop() + } finally { + inProgress.set(false) + } + } + atomicEnabled.set(false) + } + } else { + logger.warn { "Operation in progress, Kinect1 device: $num, requested enabled=$value" } + } } - } + } private fun start() { logger.info { "Enabling Kinect1 depth camera, device num: $num" } verify(freenect_set_depth_mode( fnDev, freenect_find_depth_mode(FREENECT_RESOLUTION_MEDIUM, FREENECT_DEPTH_11BIT)) ) - verify(freenect_set_depth_buffer(fnDev, Pointer(depthCamera.bytes))) - freenect_set_depth_callback(fnDev, depthCamera.freenectDepthCb) + verify(freenect_set_depth_buffer(fnDev, Pointer(bytes))) verify(freenect_start_depth(fnDev)) + Thread.sleep(depthCameraInitializationDelay) // here is the hack + freenect_set_depth_callback(fnDev, freenectDepthCb) } private fun stop() { logger.info { "Disabling Kinect1 depth camera, device num: $num" } @@ -88,62 +222,17 @@ class KinectsV1Manager : KinectsManager { } } - override fun init() { - logger.info("Initializing Kinect1 support, set log level to TRACE to see received frames") - verify(freenect_init(fnCtx, fnUsbCtx)) - freenect_set_log_level(fnCtx, FREENECT_LOG_DEBUG) - freenect_select_subdevices(fnCtx, FREENECT_DEVICE_CAMERA) - val num = verify(freenect_num_devices(fnCtx)) - if (num == 0) { - logger.warn { "Could not find any Kinect1 device, calling startDevice() will throw exception" } - } - logger.debug("Initializing Kinect1 poller thread") - poller.start() - // it seems that we have to wait a bit until kinect is actually initialized - Thread.sleep(100) - } - - override fun countDevices(): Int { - return verify(freenect_num_devices(fnCtx)) - } - - override fun startDevice(num: Int): KinectDevice { - val count = countDevices() - if (num >= count) { - throw KinectException("Non-existent Kinect1 device, num: $num") - } - val device = KinectV1Device(num) - devices.add(device) - return device - } - - override fun shutdown() { - logger.info("Shutting down Kinect1 support") - running = false - poller.join() - devices.forEach { - device -> device.shutdown() - } - if (!fnCtx.isNull) { - verifyOnShutdown(freenect_shutdown(fnCtx)) - } - } - private fun verifyOnShutdown(ret: Int) { if (ret != 0) { - logger.error("Unexpected return value while shutting down kinect support: {}", ret) + logger.error { "Unexpected return value while shutting down Kinect1 support: $ret" } } } private fun verify(ret: Int): Int { if (ret < 0) { - fail("ret=$ret") + throw KinectException("Kinect1 error: ret=$ret") } return ret } - private fun fail(message: String) { - throw KinectException("Kinect1 error: $message") - } - }