From 182ff7e6a17742a65829a7c7e7d61f6982eeb374 Mon Sep 17 00:00:00 2001
From: Edwin Jakobs <edwin@rndr.studio>
Date: Tue, 17 Nov 2020 06:18:18 -0800
Subject: [PATCH] [orx-realsense2] Fix support for multiple sensors

---
 orx-realsense2/src/demo/kotlin/DemoRS202.kt | 45 +++++++++++++++++++++
 orx-realsense2/src/main/kotlin/RS2Sensor.kt | 20 ++++++++-
 2 files changed, 64 insertions(+), 1 deletion(-)
 create mode 100644 orx-realsense2/src/demo/kotlin/DemoRS202.kt

diff --git a/orx-realsense2/src/demo/kotlin/DemoRS202.kt b/orx-realsense2/src/demo/kotlin/DemoRS202.kt
new file mode 100644
index 00000000..9f991a5f
--- /dev/null
+++ b/orx-realsense2/src/demo/kotlin/DemoRS202.kt
@@ -0,0 +1,45 @@
+import org.openrndr.application
+import org.openrndr.color.ColorRGBa
+import org.openrndr.draw.ColorFormat
+import org.openrndr.draw.ColorType
+import org.openrndr.draw.colorBuffer
+import org.openrndr.draw.tint
+import org.openrndr.extra.realsense2.RS2Sensor
+
+/**
+ * show how to use multiple RealSense sensors
+ *
+ * Tested with two sensors, only uses depth stream now
+ */
+fun main() {
+    application {
+        configure {
+            width = 1280
+            height = 720
+        }
+        program {
+            val sensorDescriptions = RS2Sensor.listSensors()
+
+            val sensors = sensorDescriptions.map {
+                it.open()
+            }
+
+            val depthFrames = sensors.map {
+                colorBuffer(640, 480, format = ColorFormat.R, type = ColorType.UINT16)
+            }
+            sensors.forEachIndexed { index, it ->
+                it.depthFrameReceived.listen {
+                    it.copyTo(depthFrames[index])
+                }
+            }
+            extend {
+                drawer.drawStyle.colorMatrix = tint(ColorRGBa.WHITE.shade(20.0))
+                for ((index, sensor) in sensors.withIndex()) {
+                    sensor.waitForFrames()
+                    drawer.image(depthFrames[index])
+                    drawer.translate(640.0, 0.0)
+                }
+            }
+        }
+    }
+}
\ No newline at end of file
diff --git a/orx-realsense2/src/main/kotlin/RS2Sensor.kt b/orx-realsense2/src/main/kotlin/RS2Sensor.kt
index 177a72f9..7ea7dac7 100644
--- a/orx-realsense2/src/main/kotlin/RS2Sensor.kt
+++ b/orx-realsense2/src/main/kotlin/RS2Sensor.kt
@@ -26,7 +26,7 @@ private fun rs2_error.check() {
 enum class RS2DepthFormat {
     UINT16
 }
-class RS2SensorDescription(private val deviceList: rs2_device_list, private val deviceIndex: Int) {
+data class RS2SensorDescription(private val deviceList: rs2_device_list, private val deviceIndex: Int) {
     /**
      * open realsense sensor from description entry
      */
@@ -47,6 +47,9 @@ class RS2SensorDescription(private val deviceList: rs2_device_list, private val
         val config = rs2_create_config(error)
         error.check()
 
+        val info = rs2_get_device_info(device, RS2_CAMERA_INFO_SERIAL_NUMBER, error)
+        rs2_config_enable_device(config, info, error)
+
         rs2_config_enable_stream(config, RS2_STREAM_DEPTH, 0, depthWidth, depthHeight, RS2_FORMAT_Z16, depthFps, error)
         error.check()
 
@@ -128,6 +131,9 @@ abstract class Sensor {
      */
     val depthFrameReceived = Event<RS2FrameEvent>()
 
+
+    abstract val serial: String
+
     /**
      * a list of [Stream]s for the [Sensor]
      */
@@ -145,6 +151,8 @@ abstract class Sensor {
 }
 
 class DummySensor : Sensor() {
+    override val serial: String = "DummySensor-${System.identityHashCode(this)}"
+
     override fun waitForFrames() {
     }
 
@@ -161,6 +169,16 @@ class RS2Sensor(
         private val pipelineProfile: rs2_pipeline_profile
 
 ) : Sensor() {
+
+    override val serial: String by lazy {
+        val error = rs2_error()
+        val info = rs2_get_device_info(device, RS2_CAMERA_INFO_SERIAL_NUMBER, error)
+        val serial = info.string
+        error.close()
+        info.close()
+        serial
+    }
+
     override fun waitForFrames() {
         val error = rs2_error()
         val frames = rs2_pipeline_wait_for_frames(pipeline, RS2_DEFAULT_TIMEOUT, error)