Add script to pull IMU and Camera data from OAK-D Camera (#331)

ewfuentes · Nov 1, 2024 · 0da047c · 0da047c
1 parent f71f222
commit 0da047c
Show file tree

Hide file tree

Showing 5 changed files with 176 additions and 1 deletion.
diff --git a/experimental/oakd_interface/BUILD b/experimental/oakd_interface/BUILD
@@ -0,0 +1,14 @@
+
+load("@pip//:requirements.bzl", "requirement")
+load("@rules_python//python:defs.bzl", "py_binary")
+
+py_binary(
+    name = "oakd_interface",
+    srcs = ["oakd_interface.py"],
+    deps = [
+	requirement("depthai"),
+	requirement("numpy"),
+    ]
+)
+
+
diff --git a/experimental/oakd_interface/oakd_interface.py b/experimental/oakd_interface/oakd_interface.py
@@ -0,0 +1,81 @@
+
+import depthai
+import numpy as np
+import datetime
+
+def main():
+    pipeline = depthai.Pipeline()
+
+    # Setup up the IMU
+    imu = pipeline.create(depthai.node.IMU)
+    imu_out = pipeline.create(depthai.node.XLinkOut)
+
+    imu_out.setStreamName("imu")
+    imu.enableIMUSensor(depthai.IMUSensor.ACCELEROMETER_RAW, 250)
+    imu.enableIMUSensor(depthai.IMUSensor.GYROSCOPE_RAW, 200)
+
+    imu.setBatchReportThreshold(5)
+    imu.setMaxBatchReports(100)
+
+    imu.out.link(imu_out.input)
+
+    # Setup the main color camera
+
+    cam_rgb = pipeline.create(depthai.node.Camera)
+
+    cam_rgb.setBoardSocket(depthai.CameraBoardSocket.CAM_A)
+    cam_rgb.setFps(15.0)
+    cam_rgb.setSize(1280, 720)
+    cam_rgb.initialControl.setFrameSyncMode(depthai.RawCameraControl.FrameSyncMode.INPUT)
+
+    # Setup up the left and right cameras
+    cam_left = pipeline.create(depthai.node.Camera)
+    cam_left.setBoardSocket(depthai.CameraBoardSocket.CAM_B)
+    cam_left.setFps(15.0)
+    cam_left.setSize(640, 480)
+    cam_left.initialControl.setFrameSyncMode(depthai.RawCameraControl.FrameSyncMode.INPUT)
+
+    cam_right = pipeline.create(depthai.node.Camera)
+    cam_right.setBoardSocket(depthai.CameraBoardSocket.CAM_C)
+    cam_right.setFps(15.0)
+    cam_right.setSize(640, 480)
+    cam_right.initialControl.setFrameSyncMode(depthai.RawCameraControl.FrameSyncMode.OUTPUT)
+
+
+    # Sync the camera images together
+    cam_sync = pipeline.create(depthai.node.Sync)
+    cam_rgb.video.link(cam_sync.inputs['rgb'])
+    cam_left.video.link(cam_sync.inputs['left'])
+    cam_right.video.link(cam_sync.inputs['right'])
+    cam_sync.setSyncThreshold(datetime.timedelta(milliseconds=10))
+
+    cam_out = pipeline.create(depthai.node.XLinkOut)
+    cam_out.setStreamName('cam')
+    cam_sync.out.link(cam_out.input)
+
+    with depthai.Device(pipeline) as device:
+        imu_queue = device.getOutputQueue(name = 'imu', maxSize=50,  blocking=True)
+        cam_queue = device.getOutputQueue(name = 'cam', maxSize=10,  blocking=False)
+
+        while True:
+            cam_data = cam_queue.tryGet()
+            imu_data = imu_queue.get()
+            # print('Received IMU Packet:', imu_data, len(imu_data.packets))
+            # print('cam_data', cam_data)
+
+            for packet in imu_data.packets:
+                accel = packet.acceleroMeter
+                accel_t = accel.getTimestamp()
+
+                gyro = packet.gyroscope
+                gyro_t = gyro.getTimestamp()
+                # print([accel.sequence, accel.x, accel.y, accel.z], accel_t,
+                #       [gyro.sequence, gyro.x, gyro.y, gyro.z], gyro_t)
+
+            if cam_data:
+                print(cam_data.getMessageNames(), 
+                      [(name, data.getSequenceNum())for name, data in cam_data],
+                      cam_data.getTimestamp(), cam_data.getIntervalNs())
+
+if __name__ == "__main__":
+    main()
diff --git a/third_party/python/requirements.in b/third_party/python/requirements.in
@@ -20,4 +20,4 @@ click==8.1.7
 spatialmath-python==1.1.8
 reportlab==4.1.0
 seaborn==0.13.2
-
+depthai==2.28.0.0
diff --git a/third_party/python/requirements_3_10.txt b/third_party/python/requirements_3_10.txt
@@ -322,6 +322,46 @@ defusedxml==0.7.1 \
     --hash=sha256:1bb3032db185915b62d7c6209c5a8792be6a32ab2fedacc84e01b52c51aa3e69 \
     --hash=sha256:a352e7e428770286cc899e2542b6cdaedb2b4953ff269a210103ec58f6198a61
     # via nbconvert
+depthai==2.28.0.0 \
+    --hash=sha256:00a134ac521f41a8e51005ce912ad195595258cd343bb371cbdf5c8bf35e7bb3 \
+    --hash=sha256:0530886985f8e2f9df2d6c5640f46e0f34100d318e11742dfa119bddf62d040f \
+    --hash=sha256:0f068130a102fbcbeef6cc23d253b0e78e8697454a1aad22eaa294afc527c4c3 \
+    --hash=sha256:0facf6602fbcddd32205189346a49f1cb6e818773baebdc5926c932da860eb54 \
+    --hash=sha256:1041f6034bece5792c5b864d8d6d2bd626d2d4c68e60ed75de8e04bd92b1d8e8 \
+    --hash=sha256:146bfcc7e4b6fd8b4f2137024446a4ce3d1bff3082c5261c78a5687ce25a7b1c \
+    --hash=sha256:14ebb4d3ab8d44c233559310cf5b0f99b41e3f813c4a2de79a06855fb30d83bc \
+    --hash=sha256:18ce8aff5a29a9dd2574da5f042676a64a02a5240b86bf5f76825af05c51c116 \
+    --hash=sha256:2145ac923186884f32e411c9c7fbd32973c352fc7f8f422012932010dd976e70 \
+    --hash=sha256:2713d844461a8d194838a9d4b8b3373ce05985dd343ade6ed135f2d73a4bf2f7 \
+    --hash=sha256:4de4af436b19514e4842932055c041130852cd2e81e89704d729cfbd700a1c9f \
+    --hash=sha256:551925ad157f396d7de349fdf654a0c02eda434a7a5c78784d5ea7eb729b8419 \
+    --hash=sha256:5c4f4b1edf9e149d4793d5d7faea91ad9fbece863897a418fb126065f3531061 \
+    --hash=sha256:5cb22354b2b1ac0225d26ac6276bfdc2acb959a7c96324fb1a045a3787e988af \
+    --hash=sha256:7674aa5985413f049538ea074901d8d0252ff908d969d8fa41a6b6eed2e8dcb0 \
+    --hash=sha256:8967876a8a42af9b97ba23abf60efad75367b22551a36d0846d553c842ab45e2 \
+    --hash=sha256:8afc65b0e2800c37ae1f1ddd38d0716b70dcc77df8393a0bfc9d00e6595563e1 \
+    --hash=sha256:8c385cd8c2602bb0c4ddc4f8d9134eead42e7f63eecfee9a10736e73f7340f2a \
+    --hash=sha256:8d40e7a3d8be8d18611f472163c119316a9f8504495292bab798e636a06a4854 \
+    --hash=sha256:94e4c8c4cda896020a9754558438d0c4977cbdee859428ad5bdf8f3c5ce22b47 \
+    --hash=sha256:ab6e2ff0e05a2f208c5be35ead3146267bf85f11669341b825c1a1c2ee808373 \
+    --hash=sha256:abea6184c0d3967307824553a0d88759586321e43f018545ab64b0b8135e8a45 \
+    --hash=sha256:b894c8168bfc29df378c2eb1d53e4fb1971ea65f27128270c076f8709dd82515 \
+    --hash=sha256:bb3ecab0ec22d9f4bdf00c0bac9cce09d3574d3c9b396624808c28a0c0a179aa \
+    --hash=sha256:c59c91870cd07101089986a635bf5de18dd777e3ba7963507b7d653073f97c90 \
+    --hash=sha256:c68992725ac40d3f088c856191a68f604b37f66f11720dd1e09337ceb69d18fd \
+    --hash=sha256:c912cc5a6582d02a07f59004a475c605ac94cc4efacac2747ea32a16782b7a8b \
+    --hash=sha256:cad79a6665ede5adf911e5ccfdd8c58f197a88c2689b3dff4bb8f8917288b714 \
+    --hash=sha256:cc7e961f966da684e876df29444a5eb383911ddc319382be388ee06c29fdce48 \
+    --hash=sha256:d3510af34f20974ea707dff691946bc37098ad3b61380ed9e1eb72b9f3f204c9 \
+    --hash=sha256:dcbe2c1d0b7b9dfa2ea00548b7d8013906507f835023366fe1182d227c6b0912 \
+    --hash=sha256:e7921ec5ebf4a02994ba7c8cb16849e2c65c6e7663a56ff2c4357f8c67f4db52 \
+    --hash=sha256:edee79d72b3a27deb2ff10d7d9fc9d2aa2e05eec9db3590dc0594177444bffa2 \
+    --hash=sha256:ef7e66421f3b4390edbe3ad72cbd523ef7d29881bab79c89a557537851a96df9 \
+    --hash=sha256:f342fb3c791ef0e4c5db80027290a406cf1add059f9ce68bf06ab1819a9c6b9e \
+    --hash=sha256:f4c3fc94759ad60626883b462d18b2a818798d51a3f9aa7585cb6e53083dd5fb \
+    --hash=sha256:fcfd087bcd09a83914ae9e6d421923c1f4f25341aeee40f23575282b0ebd70e4 \
+    --hash=sha256:ff4d59a9bbb39e1673a74e3a7a1b6e0a5df663b2e708da3f04f04b4794dc1c52
+    # via -r third_party/python/requirements.in
 exceptiongroup==1.2.0 \
     --hash=sha256:4bfd3996ac73b41e9b9628b04e079f193850720ea5945fc96a08633c66912f14 \
     --hash=sha256:91f5c769735f051a4290d52edd0858999b57e5876e9f85937691bd4c9fa3ed68

diff --git a/third_party/python/requirements_3_8.txt b/third_party/python/requirements_3_8.txt
@@ -334,6 +334,46 @@ defusedxml==0.7.1 \
     --hash=sha256:1bb3032db185915b62d7c6209c5a8792be6a32ab2fedacc84e01b52c51aa3e69 \
     --hash=sha256:a352e7e428770286cc899e2542b6cdaedb2b4953ff269a210103ec58f6198a61
     # via nbconvert
+depthai==2.28.0.0 \
+    --hash=sha256:00a134ac521f41a8e51005ce912ad195595258cd343bb371cbdf5c8bf35e7bb3 \
+    --hash=sha256:0530886985f8e2f9df2d6c5640f46e0f34100d318e11742dfa119bddf62d040f \
+    --hash=sha256:0f068130a102fbcbeef6cc23d253b0e78e8697454a1aad22eaa294afc527c4c3 \
+    --hash=sha256:0facf6602fbcddd32205189346a49f1cb6e818773baebdc5926c932da860eb54 \
+    --hash=sha256:1041f6034bece5792c5b864d8d6d2bd626d2d4c68e60ed75de8e04bd92b1d8e8 \
+    --hash=sha256:146bfcc7e4b6fd8b4f2137024446a4ce3d1bff3082c5261c78a5687ce25a7b1c \
+    --hash=sha256:14ebb4d3ab8d44c233559310cf5b0f99b41e3f813c4a2de79a06855fb30d83bc \
+    --hash=sha256:18ce8aff5a29a9dd2574da5f042676a64a02a5240b86bf5f76825af05c51c116 \
+    --hash=sha256:2145ac923186884f32e411c9c7fbd32973c352fc7f8f422012932010dd976e70 \
+    --hash=sha256:2713d844461a8d194838a9d4b8b3373ce05985dd343ade6ed135f2d73a4bf2f7 \
+    --hash=sha256:4de4af436b19514e4842932055c041130852cd2e81e89704d729cfbd700a1c9f \
+    --hash=sha256:551925ad157f396d7de349fdf654a0c02eda434a7a5c78784d5ea7eb729b8419 \
+    --hash=sha256:5c4f4b1edf9e149d4793d5d7faea91ad9fbece863897a418fb126065f3531061 \
+    --hash=sha256:5cb22354b2b1ac0225d26ac6276bfdc2acb959a7c96324fb1a045a3787e988af \
+    --hash=sha256:7674aa5985413f049538ea074901d8d0252ff908d969d8fa41a6b6eed2e8dcb0 \
+    --hash=sha256:8967876a8a42af9b97ba23abf60efad75367b22551a36d0846d553c842ab45e2 \
+    --hash=sha256:8afc65b0e2800c37ae1f1ddd38d0716b70dcc77df8393a0bfc9d00e6595563e1 \
+    --hash=sha256:8c385cd8c2602bb0c4ddc4f8d9134eead42e7f63eecfee9a10736e73f7340f2a \
+    --hash=sha256:8d40e7a3d8be8d18611f472163c119316a9f8504495292bab798e636a06a4854 \
+    --hash=sha256:94e4c8c4cda896020a9754558438d0c4977cbdee859428ad5bdf8f3c5ce22b47 \
+    --hash=sha256:ab6e2ff0e05a2f208c5be35ead3146267bf85f11669341b825c1a1c2ee808373 \
+    --hash=sha256:abea6184c0d3967307824553a0d88759586321e43f018545ab64b0b8135e8a45 \
+    --hash=sha256:b894c8168bfc29df378c2eb1d53e4fb1971ea65f27128270c076f8709dd82515 \
+    --hash=sha256:bb3ecab0ec22d9f4bdf00c0bac9cce09d3574d3c9b396624808c28a0c0a179aa \
+    --hash=sha256:c59c91870cd07101089986a635bf5de18dd777e3ba7963507b7d653073f97c90 \
+    --hash=sha256:c68992725ac40d3f088c856191a68f604b37f66f11720dd1e09337ceb69d18fd \
+    --hash=sha256:c912cc5a6582d02a07f59004a475c605ac94cc4efacac2747ea32a16782b7a8b \
+    --hash=sha256:cad79a6665ede5adf911e5ccfdd8c58f197a88c2689b3dff4bb8f8917288b714 \
+    --hash=sha256:cc7e961f966da684e876df29444a5eb383911ddc319382be388ee06c29fdce48 \
+    --hash=sha256:d3510af34f20974ea707dff691946bc37098ad3b61380ed9e1eb72b9f3f204c9 \
+    --hash=sha256:dcbe2c1d0b7b9dfa2ea00548b7d8013906507f835023366fe1182d227c6b0912 \
+    --hash=sha256:e7921ec5ebf4a02994ba7c8cb16849e2c65c6e7663a56ff2c4357f8c67f4db52 \
+    --hash=sha256:edee79d72b3a27deb2ff10d7d9fc9d2aa2e05eec9db3590dc0594177444bffa2 \
+    --hash=sha256:ef7e66421f3b4390edbe3ad72cbd523ef7d29881bab79c89a557537851a96df9 \
+    --hash=sha256:f342fb3c791ef0e4c5db80027290a406cf1add059f9ce68bf06ab1819a9c6b9e \
+    --hash=sha256:f4c3fc94759ad60626883b462d18b2a818798d51a3f9aa7585cb6e53083dd5fb \
+    --hash=sha256:fcfd087bcd09a83914ae9e6d421923c1f4f25341aeee40f23575282b0ebd70e4 \
+    --hash=sha256:ff4d59a9bbb39e1673a74e3a7a1b6e0a5df663b2e708da3f04f04b4794dc1c52
+    # via -r third_party/python/requirements.in
 exceptiongroup==1.2.0 \
     --hash=sha256:4bfd3996ac73b41e9b9628b04e079f193850720ea5945fc96a08633c66912f14 \
     --hash=sha256:91f5c769735f051a4290d52edd0858999b57e5876e9f85937691bd4c9fa3ed68