diff --git a/orx-kinect-v1/src/main/kotlin/KinectV1.kt b/orx-kinect-v1/src/main/kotlin/KinectV1.kt index 96176101..41798ece 100644 --- a/orx-kinect-v1/src/main/kotlin/KinectV1.kt +++ b/orx-kinect-v1/src/main/kotlin/KinectV1.kt @@ -5,7 +5,9 @@ import org.bytedeco.javacpp.Pointer import org.bytedeco.libfreenect.* import org.bytedeco.libfreenect.global.freenect.* import org.bytedeco.libfreenect.presets.freenect +import org.openrndr.Program import org.openrndr.extra.kinect.* +import org.openrndr.extra.kinect.impl.* import java.nio.ByteBuffer import java.nio.ByteOrder import java.util.* @@ -13,6 +15,7 @@ import java.util.concurrent.* import java.util.concurrent.atomic.AtomicBoolean import java.util.concurrent.atomic.AtomicReference import java.util.function.Supplier +import kotlin.concurrent.thread /** * Returns support for kinect version 1. @@ -25,8 +28,8 @@ import java.util.function.Supplier * kinect. Subsequent runs of the same program don't require * this delay at all. */ -fun getKinectsV1(depthCameraInitializationDelay: Long = 100) : Kinects { - return DefaultKinects(KinectsV1Manager(depthCameraInitializationDelay)) +fun getKinectsV1(program: Program, depthCameraInitializationDelay: Long = 100) : Kinects { + return DefaultKinects(program, KinectsV1Manager(depthCameraInitializationDelay)) } /** Provides low level freenect context for calling native freenect methods. */ @@ -46,23 +49,33 @@ private class KinectsV1Manager(val depthCameraInitializationDelay: Long) : Kinec private val ctx = Freenect(fnCtx, fnUsbCtx) + private var taskQueue = LinkedBlockingDeque>() + private var running = true + private val runner = thread( + name = "Kinect1-runner", + start = false, + isDaemon = true + ) { + initializeFreenect() + while (running) { mainLoop() } + shutdownFreenect() + } + + private var expectingEvents = false + 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") - } - - 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" } + private inner class KinectV1CommandsExecutor(val context: Freenect): KinectCommandsExecutor { + override fun execute(commands: (Freenect) -> T): T { + return callSync { + logger.trace { "executing native freenect commands" } commands(context) - }).get() + } } } @@ -70,49 +83,65 @@ private class KinectsV1Manager(val depthCameraInitializationDelay: Long) : Kinec 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" } - } + runner.start() + } + + private fun initializeFreenect() { + 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 + } + + private fun mainLoop() { + if (expectingEvents) { + val ret = freenect_process_events(fnCtx) + if (ret != 0) { logger.error { "freenect_process_events returned non-zero value: $ret" } } + val tasks = taskQueue.iterator() + for (task in tasks) { + tasks.remove() + task.run() } - }) + } else { + taskQueue.poll(100, TimeUnit.MILLISECONDS)?.run() + } + } + + private fun shutdownFreenect() { + logger.debug("shutting down freenect") + if (!fnCtx.isNull) { + devices.forEach { device -> device.shutdown() } + devices.clear() + verifyOnShutdown(freenect_shutdown(fnCtx)) + } } override fun countDevices(): Int { - return executor.submit( - Callable { verify(freenect_num_devices(fnCtx)) } - ).get() + return callSync { verify(freenect_num_devices(fnCtx)) } } - // FIXME we should prevent from starting the same device multiple times override fun startDevice(num: Int): KinectDevice { + callSync { + devices.find { device -> device.num == num } + }?.let { + throw KinectException("Kinect1 device already started, num: $num") + } val count = countDevices() if (num >= count) { throw KinectException( - "Trying to start non-existent Kinect1 device, device count: $count, num: $num" + "Trying to start non-existent Kinect1 device, " + + "device count: $count, num: $num (index starts with 0)" ) } - val device = executor.submit( - Callable { - val device = FreenectDevice(num) - devices.add(device) - device - } - ).get() + val device = callSync { + val device = FreenectDevice(num) + devices.add(device) + device + } return DefaultKinectDevice( DefaultKinectDepthCamera( device.depthCamera.width, @@ -125,32 +154,38 @@ private class KinectsV1Manager(val depthCameraInitializationDelay: Long) : Kinec ) } - override fun execute(commands: (Freenect) -> Any): Any { + override fun execute(commands: (Freenect) -> T): T { 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 + callSync { running = false } + runner.join() } - private inner class FreenectDevice(private val num: Int) { + private inline fun callSync(crossinline block: () -> T): T { + val task = FutureTask(Callable { block() }) + taskQueue.add(task) + return task.get() + } + + private inner class FreenectDevice(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)) } + + val expectingEvents: Boolean + get() = depthCamera.expectingEvents // or other device in the future + fun shutdown() { logger.info { "Shutting down Kinect1 device num: $num" } if (!fnDev.isNull) { @@ -158,23 +193,32 @@ private class KinectsV1Manager(val depthCameraInitializationDelay: Long) : Kinec verifyOnShutdown(freenect_close_device(fnDev)) } } + 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 currentBytesRef = AtomicReference() + 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" } currentBytesRef.set(bytes) } } + val bytesSupplier = Supplier { currentBytesRef.getAndSet(null) } + val enabler = object : KinectFeatureEnabler { + private val atomicEnabled = AtomicBoolean(false) private val inProgress = AtomicBoolean(false) - override var enabled + + override var enabled // usually called from rendering thread get() = atomicEnabled.get() set(value) { if (atomicEnabled.get() == value) { @@ -183,29 +227,31 @@ private class KinectsV1Manager(val depthCameraInitializationDelay: Long) : Kinec } if (!inProgress.getAndSet(true)) { if (value) { - executor.execute { + callSync { try { start() - } finally { - inProgress.set(false) - } + atomicEnabled.set(true) + updateExpectingEvents() + } finally { inProgress.set(false) } } - atomicEnabled.set(true) } else { - executor.execute { + callSync { try { stop() - } finally { - inProgress.set(false) - } + atomicEnabled.set(false) + updateExpectingEvents() + } finally { inProgress.set(false) } } - atomicEnabled.set(false) } } else { logger.warn { "Operation in progress, Kinect1 device: $num, requested enabled=$value" } } } } + + val expectingEvents: Boolean + get() = depthCamera.enabler.enabled + private fun start() { logger.info { "Enabling Kinect1 depth camera, device num: $num" } verify(freenect_set_depth_mode( @@ -216,6 +262,7 @@ private class KinectsV1Manager(val depthCameraInitializationDelay: Long) : Kinec 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" } verify(freenect_stop_depth(fnDev)) @@ -223,6 +270,10 @@ private class KinectsV1Manager(val depthCameraInitializationDelay: Long) : Kinec } } + private fun updateExpectingEvents() { + expectingEvents = devices.any { device -> device.expectingEvents } + } + private fun verifyOnShutdown(ret: Int) { if (ret != 0) { logger.error { "Unexpected return value while shutting down Kinect1 support: $ret" }