Merge branch 'main' into disparity_shift

Author: Erol444

depthai-core

@@ -0,0 +1,53 @@
+Frame syncing on OAK
+====================
+
+This example showcases how you can use :ref:`Script` node to sync frames from multiple streams. It uses :ref:`ImgFrame`'s timestamps to achieve syncing precision.
+
+Similar syncing demo scripts (python) can be found at our depthai-experiments repository in `gen2-syncing <https://github.com/luxonis/depthai-experiments/tree/master/gen2-syncing>`__
+folder.
+
+Demo
+####
+
+Terminal log after about 13 minutes. Color and disparity streams are perfectly in-sync.
+
+.. code-block:: bash
+
+[1662574807.8811488] Stream rgb, timestamp: 7:26:21.601595, sequence number: 21852
+[1662574807.8821492] Stream disp, timestamp: 7:26:21.601401, sequence number: 21852
+
+[1662574807.913144] Stream rgb, timestamp: 7:26:21.634982, sequence number: 21853
+[1662574807.9141443] Stream disp, timestamp: 7:26:21.634730, sequence number: 21853
+
+[1662574807.9451444] Stream rgb, timestamp: 7:26:21.668243, sequence number: 21854
+[1662574807.946151] Stream disp, timestamp: 7:26:21.668057, sequence number: 21854
+
+Setup
+#####
+
+.. include::  /includes/install_from_pypi.rst
+
+.. include:: /includes/install_req.rst
+
+Source code
+###########
+
+.. tabs::
+
+    .. tab:: Python
+
+        Also `available on GitHub <https://github.com/luxonis/depthai-python/blob/main/examples/mixed/frame_sync.py>`__
+
+        .. literalinclude:: ../../../../examples/mixed/frame_sync.py
+           :language: python
+           :linenos:
+
+    .. tab:: C++
+
+        Also `available on GitHub <https://github.com/luxonis/depthai-core/blob/main/examples/mixed/frame_sync.cpp>`__
+
+        .. literalinclude:: ../../../../depthai-core/examples/mixed/frame_sync.cpp
+           :language: cpp
+           :linenos:
+
+.. include::  /includes/footer-short.rst

docs/source/samples/mixed/frame_sync.rst

@@ -0,0 +1,113 @@
+import depthai as dai
+import time
+
+FPS = 30
+
+pipeline = dai.Pipeline()
+
+# Define a source - color camera
+camRgb = pipeline.create(dai.node.ColorCamera)
+# Since we are saving RGB frames in Script node we need to make the
+# video pool size larger, otherwise the pipeline will freeze because
+# the ColorCamera won't be able to produce new video frames.
+camRgb.setVideoNumFramesPool(10)
+camRgb.setFps(FPS)
+
+left = pipeline.create(dai.node.MonoCamera)
+left.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
+left.setBoardSocket(dai.CameraBoardSocket.LEFT)
+left.setFps(FPS)
+
+right = pipeline.create(dai.node.MonoCamera)
+right.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
+right.setBoardSocket(dai.CameraBoardSocket.RIGHT)
+right.setFps(FPS)
+
+stereo = pipeline.createStereoDepth()
+stereo.initialConfig.setMedianFilter(dai.MedianFilter.KERNEL_7x7)
+stereo.setLeftRightCheck(True)
+stereo.setExtendedDisparity(False)
+stereo.setSubpixel(False)
+left.out.link(stereo.left)
+right.out.link(stereo.right)
+
+# Script node will sync high-res frames
+script = pipeline.create(dai.node.Script)
+
+# Send both streams to the Script node so we can sync them
+stereo.disparity.link(script.inputs["disp_in"])
+camRgb.video.link(script.inputs["rgb_in"])
+
+script.setScript("""
+    FPS=30
+    import time
+    from datetime import timedelta
+    import math
+
+    # Timestamp threshold (in miliseconds) under which frames will be considered synced.
+    # Lower number means frames will have less delay between them, which can potentially
+    # lead to dropped frames.
+    MS_THRESHOL=math.ceil(500 / FPS)
+
+    def check_sync(queues, timestamp):
+        matching_frames = []
+        for name, list in queues.items(): # Go through each available stream
+            # node.warn(f"List {name}, len {str(len(list))}")
+            for i, msg in enumerate(list): # Go through each frame of this stream
+                time_diff = abs(msg.getTimestamp() - timestamp)
+                if time_diff <= timedelta(milliseconds=MS_THRESHOL): # If time diff is below threshold, this frame is considered in-sync
+                    matching_frames.append(i) # Append the position of the synced frame, so we can later remove all older frames
+                    break
+
+        if len(matching_frames) == len(queues):
+            # We have all frames synced. Remove the excess ones
+            i = 0
+            for name, list in queues.items():
+                queues[name] = queues[name][matching_frames[i]:] # Remove older (excess) frames
+                i+=1
+            return True
+        else:
+            return False # We don't have synced frames yet
+
+    names = ['disp', 'rgb']
+    frames = dict() # Dict where we store all received frames
+    for name in names:
+        frames[name] = []
+
+    while True:
+        for name in names:
+            f = node.io[name+"_in"].tryGet()
+            if f is not None:
+                frames[name].append(f) # Save received frame
+
+                if check_sync(frames, f.getTimestamp()): # Check if we have any synced frames
+                    # Frames synced!
+                    node.info(f"Synced frame!")
+                    # node.warn(f"Queue size. Disp: {len(frames['disp'])}, rgb: {len(frames['rgb'])}")
+                    for name, list in frames.items():
+                        syncedF = list.pop(0) # We have removed older (excess) frames, so at positions 0 in dict we have synced frames
+                        node.info(f"{name}, ts: {str(syncedF.getTimestamp())}, seq {str(syncedF.getSequenceNum())}")
+                        node.io[name+'_out'].send(syncedF) # Send synced frames to the host
+
+
+        time.sleep(0.001)  # Avoid lazy looping
+""")
+
+script_out = ['disp', 'rgb']
+
+for name in script_out: # Create XLinkOut for disp/rgb streams
+    xout = pipeline.create(dai.node.XLinkOut)
+    xout.setStreamName(name)
+    script.outputs[name+'_out'].link(xout.input)
+
+with dai.Device(pipeline) as device:
+    device.setLogLevel(dai.LogLevel.INFO)
+    device.setLogOutputLevel(dai.LogLevel.INFO)
+    names = ['rgb', 'disp']
+    queues = [device.getOutputQueue(name) for name in names]
+
+    while True:
+        for q in queues:
+            img: dai.ImgFrame = q.get()
+            # Display timestamp/sequence number of two synced frames
+            print(f"Time: {time.time()}. Stream {q.getName()}, timestamp: {img.getTimestamp()}, sequence number: {img.getSequenceNum()}")

examples/mixed/frame_sync.py

@@ -54,12 +54,12 @@ def check_mac(s):
     return True
 
 class Progress:
-    def __init__(self):
+    def __init__(self, txt = 'Flashing progress: 0.0%'):
         layout = [
-            [sg.Text('Flashing progress: 0.0%', key='txt')],
+            [sg.Text(txt, key='txt')],
             [sg.ProgressBar(1.0, orientation='h', size=(20,20), key='progress')],
         ]
-        self.self.window = sg.Window("Progress", layout, modal=True, finalize=True)
+        self.window = sg.Window("Progress", layout, modal=True, finalize=True)
     def update(self, val):
         self.window['progress'].update(val)
         self.window['txt'].update(f'Flashing progress: {val*100:.1f}%')
@@ -83,8 +83,11 @@ def __init__(self, options, text):
     def wait(self):
         event, values = self.window.Read()
         self.window.close()
-        type = getattr(dai.DeviceBootloader.Type, values['bootType'])
-        return (str(event) == "Submit", type)
+        if values is not None:
+            type = getattr(dai.DeviceBootloader.Type, values['bootType'])
+            return (str(event) == "Submit", type)
+        else:
+            return (False, None)
 
 class SelectIP:
     def __init__(self):
@@ -158,17 +161,13 @@ def wait(self) -> dai.DeviceInfo:
                     self.window.close()
                     return deviceSelected
 
-def flashBootloader(device: dai.DeviceInfo):
+def flashBootloader(device: dai.DeviceInfo, type: dai.DeviceBootloader.Type):
     try:
-        sel = SelectBootloader(['AUTO', 'USB', 'NETWORK'], "Select bootloader type to flash.")
-        ok, type = sel.wait()
-        if not ok:
-            print("Flashing bootloader canceled.")
-            return
+
+        pr = Progress('Connecting...')
 
         bl = dai.DeviceBootloader(device, True)
 
-        pr = Progress()
         progress = lambda p : pr.update(p)
         if type == dai.DeviceBootloader.Type.AUTO:
             type = bl.getType()
@@ -178,22 +177,19 @@ def flashBootloader(device: dai.DeviceInfo):
         PrintException()
         sg.Popup(f'{ex}')
 
-def factoryReset(bl: dai.DeviceBootloader):
-    sel = SelectBootloader(['USB', 'NETWORK'], "Select bootloader type used for factory reset.")
-    ok, type = sel.wait()
-    if not ok:
-        print("Factory reset canceled.")
-        return
-
+def factoryReset(device: dai.DeviceInfo, type: dai.DeviceBootloader.Type):
     try:
+        pr = Progress('Preparing and connecting...')
+
         blBinary = dai.DeviceBootloader.getEmbeddedBootloaderBinary(type)
         # Clear 1 MiB for USB BL and 8 MiB for NETWORK BL
         mib = 1 if type == dai.DeviceBootloader.Type.USB else 8
         blBinary = blBinary + ([0xFF] * ((mib * 1024 * 1024 + 512) - len(blBinary)))
         tmpBlFw = tempfile.NamedTemporaryFile(delete=False)
         tmpBlFw.write(bytes(blBinary))
 
-        pr = Progress()
+        bl = dai.DeviceBootloader(device, True)
+
         progress = lambda p : pr.update(p)
         success, msg = bl.flashBootloader(progress, tmpBlFw.name)
         msg = "Factory reset was successful." if success else f"Factory reset failed. Error: {msg}"
@@ -216,9 +212,11 @@ def flashFromFile(file, bl: dai.DeviceBootloader):
 def recoveryMode(bl: dai.DeviceBootloader):
     try:
         bl.bootUsbRomBootloader()
+        return True
     except Exception as ex:
         PrintException()
         sg.Popup(f'{ex}')
+    return False
 
 def connectToDevice(device: dai.DeviceInfo) -> dai.DeviceBootloader:
     try:
@@ -342,9 +340,11 @@ def deviceStateTxt(state: dai.XLinkDeviceState) -> str:
     ],
     [sg.HSeparator()],
     [
-        sg.Text("", size=(10, 2)),
+        sg.Text("", size=(1, 2)),
         sg.Button("Flash configuration", size=(15, 2), font=('Arial', 10, 'bold'), disabled=True,
                   button_color='#FFA500'),
+        sg.Button("Clear configuration", size=(15, 2), font=('Arial', 10, 'bold'), disabled=True,
+                  button_color='#FFA500'),
         sg.Button("Clear flash", size=(15, 2), font=('Arial', 10, 'bold'), disabled=True,
                   button_color='#FFA500'),
         sg.Button("Flash DAP", size=(15, 2), font=('Arial', 10, 'bold'), disabled=True,
@@ -443,9 +443,33 @@ def run(self) -> None:
                     self.getConfigs()
                 self.unlockConfig()
             elif event == "Flash newest Bootloader":
-                self.closeDevice()  # We will reconnect, as we need to set allowFlashingBootloader to True
-                flashBootloader(self.device)
-                self.window.Element('currBoot').update(self.bl.getVersion())
+                sel = SelectBootloader(['AUTO', 'USB', 'NETWORK'], "Select bootloader type to flash.")
+                ok, type = sel.wait()
+                if ok:
+                    # We will reconnect, as we need to set allowFlashingBootloader to True
+                    self.closeDevice()
+                    flashBootloader(self.device, type)
+                    # Device will reboot, close previous and reset GUI
+                    self.closeDevice()
+                    self.resetGui()
+                    self.getDevices()
+                else:
+                    print("Flashing bootloader cancelled.")
+
+            elif event == "Factory reset":
+                sel = SelectBootloader(['NETWORK', 'USB'], "Select bootloader type used for factory reset.")
+                ok, type = sel.wait()
+                if ok:
+                    # We will reconnect, as we need to set allowFlashingBootloader to True
+                    self.closeDevice()
+                    factoryReset(self.device, type)
+                    # Device will reboot, close previous and reset GUI
+                    self.closeDevice()
+                    self.resetGui()
+                    self.getDevices()
+                else:
+                    print("Factory reset cancelled.")
+
             elif event == "Flash configuration":
                 self.flashConfig()
                 self.getConfigs()
@@ -455,8 +479,16 @@ def run(self) -> None:
                 else:
                     self.devices.clear()
                     self.window.Element('devices').update("Search for devices", values=[])
-            elif event == "Factory reset":
-                factoryReset(self.bl)
+            elif event == "Clear configuration":
+                self.clearConfig()
+                self.getConfigs()
+                self.resetGui()
+                if self.isUsb():
+                    self.unlockConfig()
+                else:
+                    self.devices.clear()
+                    self.window.Element('devices').update("Search for devices", values=[])
+
             elif event == "Flash DAP":
                 file = sg.popup_get_file("Select .dap file", file_types=(('DepthAI Application Package', '*.dap'), ('All Files', '*.* *')))
                 flashFromFile(file, self.bl)
@@ -467,7 +499,12 @@ def run(self) -> None:
                 self.window['-COL2-'].update(visible=False)
                 self.window['-COL1-'].update(visible=True)
             elif event == "recoveryMode":
-                recoveryMode(self.bl)
+                if recoveryMode(self.bl):
+                    sg.Popup(f'Device successfully put into USB recovery mode.')
+                # Device will reboot, close previous and reset GUI
+                self.closeDevice()
+                self.resetGui()
+                self.getDevices()
         self.window.close()
 
     @property
@@ -540,6 +577,7 @@ def unlockConfig(self):
 
         self.window['Flash newest Bootloader'].update(disabled=False)
         self.window['Flash configuration'].update(disabled=False)
+        self.window['Clear configuration'].update(disabled=False)
         self.window['Factory reset'].update(disabled=False)
         # self.window['Clear flash'].update(disabled=False)
         self.window['Flash DAP'].update(disabled=False)
@@ -559,6 +597,7 @@ def resetGui(self):
 
         self.window['Flash newest Bootloader'].update(disabled=True)
         self.window['Flash configuration'].update(disabled=True)
+        self.window['Clear configuration'].update(disabled=True)
         self.window['Factory reset'].update(disabled=True)
         self.window['Clear flash'].update(disabled=True)
         self.window['Flash DAP'].update(disabled=True)
@@ -591,7 +630,9 @@ def getDevices(self):
                     deviceTxt = deviceInfo.getMxId()
                     listedDevices.append(deviceTxt)
                     self.devices[deviceTxt] = deviceInfo
-                self.window.Element('devices').update("Select device", values=listedDevices)
+
+            # Update the list regardless
+            self.window.Element('devices').update("Select device", values=listedDevices)
         except Exception as ex:
             PrintException()
             sg.Popup(f'{ex}')
@@ -634,6 +675,17 @@ def flashConfig(self):
         except Exception as ex:
             PrintException()
             sg.Popup(f'{ex}')
+    def clearConfig(self):
+        try:
+            success, error = self.bl.flashConfigClear()
+            if not success:
+                sg.Popup(f"Clearing configuration failed: {error}")
+            else:
+                sg.Popup("Successfully cleared configuration.")
+        except Exception as ex:
+            PrintException()
+            sg.Popup(f'{ex}')
+
 
 app = DeviceManager()
 app.run()