bug and stability fixes

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 <CTX> data needed to make low level kinect support calls.
+ */
+interface Kinects<CTX> {
     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<CTX>
+
+    /**
+     * Executes low level Kinect commands in the kinect thread.
+     */
+    fun execute(commands: (CTX) -> Any) : Any
 }
 
-class DefaultKinects(private val manager: KinectsManager) : Kinects {
+/**
-    init {
+ * Represents specific device.
-        manager.init()
+ *
-        // as we don't have explicit shutdown mechanism in OPENRNDR
+ * @param <CTX> data needed to make low level kinect support calls.
-        // we need to install a shutdown hook for now
+ */
-        Runtime.getRuntime().addShutdownHook(
+interface KinectDevice<CTX> : Extension {
-            thread(
+    val depthCamera: KinectDepthCamera
-                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)
-    }
-}
 
-interface KinectsManager {
+    /**
-    fun init()
+     * Executes low level Kinect commands in the kinect thread in the context of this device.
-    fun countDevices(): Int
+     */
-    fun startDevice(num: Int): KinectDevice
+    fun execute(commands: (CTX) -> Any): Any
-    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()
-        }
-    }
 }
 
 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
+    /* no special attributes at the moment */
-}
-
-abstract class AbstractKinectDepthCamera(
-    final override val width: Int,
-    final override val height: Int,
-    final override val depthScale: Double
-) : KinectDepthCamera {
-    protected val byteBufferRef: AtomicReference<ByteBuffer> = 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<ColorBuffer>()
-    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
 }
 
 class KinectException(msg: String) : RuntimeException(msg)

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<CTX>(private val manager: KinectsManager<CTX>) : Kinects<CTX> {
+    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<CTX> {
+        return manager.startDevice(num)
+    }
+
+    override fun execute(commands: (CTX) -> Any): Any {
+        return manager.execute(commands)
+    }
+
+}
+
+interface KinectsManager<CTX> {
+    fun initialize()
+    fun countDevices(): Int
+    fun startDevice(num: Int): KinectDevice<CTX>
+    fun execute(commands: (CTX) -> Any): Any
+    fun shutdown()
+}
+
+interface KinectFeatureEnabler {
+    var enabled: Boolean
+}
+
+interface KinectCommandsExecutor<CTX> {
+    fun execute(commands: (CTX) -> Any): Any
+}
+
+class DefaultKinectDevice<CTX>(
+        override val depthCamera: DefaultKinectDepthCamera,
+        private val commandsExecutor: KinectCommandsExecutor<CTX>
+) : KinectDevice<CTX> {
+    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<ByteBuffer?>
+) :
+        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<ColorBuffer>()
+
+    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()
+}

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.*
-import org.bytedeco.libfreenect.freenect_depth_cb
-import org.bytedeco.libfreenect.freenect_device
-import org.bytedeco.libfreenect.freenect_usb_context
 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<Freenect> {
+    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<Freenect> {
 
     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<KinectV1Device> = LinkedList()
+    private var running = true
 
-    private val poller: Thread = thread(name = "Kinect1-poll", start = false, isDaemon = true) {
+    private val devices: LinkedList<FreenectDevice> = LinkedList()
-        while (running && freenect_process_events(fnCtx) >= 0) {}
+
+    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() {
+    private inner class KinectV1CommandsExecutor(val context: Freenect) : KinectCommandsExecutor<Freenect> {
-        override val depthCamera = KinectV1DepthCamera()
+        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<Freenect> {
+        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(
+        inner class FreenectDepthCamera {
-                width = 640,
+            val width: Int = 640
-                height = 480,
+            val height: Int = 480
-                depthScale = 32.0
-        ) {
-            private val logger = KotlinLogging.logger {}
             private val bytes = ByteBuffer.allocateDirect(width * height * 2)
+            private val currentBytesRef = AtomicReference<ByteBuffer?>()
+            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 {
+            val bytesSupplier = Supplier<ByteBuffer?> { currentBytesRef.getAndSet(null) }
-                bytes.order(ByteOrder.nativeOrder())
+            val enabler = object : KinectFeatureEnabler {
-            }
+                private val atomicEnabled = AtomicBoolean(false)
-            override var enabled: Boolean = false
+                private val inProgress = AtomicBoolean(false)
-                set(value) {
+                override var enabled
-                    field = if (value) {
+                    get() = atomicEnabled.get()
-                        start()
+                    set(value) {
-                        true
+                        if (atomicEnabled.get() == value) {
-                    } else {
+                            logger.warn { "Current state requested - doing nothing, Kinect1 device: $num, enabled=$value" }
-                        stop()
+                        }
-                        false
+                        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)))
+                verify(freenect_set_depth_buffer(fnDev, Pointer(bytes)))
-                freenect_set_depth_callback(fnDev, depthCamera.freenectDepthCb)
                 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")
-    }
-
 }