From 0da047cb699c41b7ce7b8e069bc9145f90990b2f Mon Sep 17 00:00:00 2001 From: Erick Fuentes Date: Fri, 1 Nov 2024 14:36:44 -0400 Subject: [PATCH] Add script to pull IMU and Camera data from OAK-D Camera (#331) --- experimental/oakd_interface/BUILD | 14 ++++ experimental/oakd_interface/oakd_interface.py | 81 +++++++++++++++++++ third_party/python/requirements.in | 2 +- third_party/python/requirements_3_10.txt | 40 +++++++++ third_party/python/requirements_3_8.txt | 40 +++++++++ 5 files changed, 176 insertions(+), 1 deletion(-) create mode 100644 experimental/oakd_interface/BUILD create mode 100644 experimental/oakd_interface/oakd_interface.py diff --git a/experimental/oakd_interface/BUILD b/experimental/oakd_interface/BUILD new file mode 100644 index 00000000..dad1fc38 --- /dev/null +++ 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 new file mode 100644 index 00000000..9408d761 --- /dev/null +++ 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 index e90890da..17a33350 100644 --- 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 index 06fa4721..7993557c 100644 --- 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 index 46ecc8e8..4214ca1a 100644 --- 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