KinectV1 fixes for performance issues

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<Freenect> {
-    return DefaultKinects(KinectsV1Manager(depthCameraInitializationDelay))
+fun getKinectsV1(program: Program, depthCameraInitializationDelay: Long = 100) : Kinects<Freenect> {
+    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<FutureTask<*>>()
+
     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<FreenectDevice> = 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<Freenect> {
-        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<Freenect> {
+        override fun <T> 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<Freenect> {
+        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 <T> 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 <T> callSync(crossinline block: () -> T): T {
+        val task = FutureTask<T>(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<ByteBuffer?>()
             init { bytes.order(ByteOrder.nativeOrder()) }
+
+            private val currentBytesRef = AtomicReference<ByteBuffer?>()
+
             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<ByteBuffer?> { 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" }