Merge pull request #1187 from luxonis/v3_imu_rvc4

Add IMU support for RVC4
luxonis · Jan 9, 2025 · 7e43b08 · 7e43b08
2 parents 1e4834a + 3e23b4a
commit 7e43b08
Show file tree

Hide file tree

Showing 5 changed files with 58 additions and 11 deletions.
diff --git a/cmake/Depthai/DepthaiDeviceRVC4Config.cmake b/cmake/Depthai/DepthaiDeviceRVC4Config.cmake
@@ -4,4 +4,4 @@ set(DEPTHAI_DEVICE_RVC4_MATURITY "snapshot")
 
 # "version if applicable"
 # set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+93f7b75a885aa32f44c5e9f53b74470c49d2b1af")
-set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+6fc71e8674fe7a520b93f4370cb157805f0bc0f2")
+set(DEPTHAI_DEVICE_RVC4_VERSION "0.0.1+670797e3b8cbc185c7d457e24382495200486573")
diff --git a/examples/python/CMakeLists.txt b/examples/python/CMakeLists.txt
@@ -237,3 +237,7 @@ set_tests_properties(py_benchmark_cameras PROPERTIES FAIL_REGULAR_EXPRESSION "\\
 
 add_python_example(benchmark_nn Benchmark/benchmark_nn.py)
 dai_set_example_test_labels(benchmark_nn ondevice rvc2_all rvc4 ci)
+
+# IMU node
+add_python_example(imu_gyroscope_accelerometer IMU/imu_gyroscope_accelerometer.py)
+dai_set_example_test_labels(imu_gyroscope_accelerometer rvc2_all rvc4 ci)
diff --git a/...n/RVC2/IMU/imu_gyroscope_accelerometer.py → ...python/IMU/imu_gyroscope_accelerometer.py b/...n/RVC2/IMU/imu_gyroscope_accelerometer.py → ...python/IMU/imu_gyroscope_accelerometer.py
@@ -7,7 +7,7 @@
     imu = pipeline.create(dai.node.IMU)
 
     # enable ACCELEROMETER_RAW at 500 hz rate
-    imu.enableIMUSensor(dai.IMUSensor.ACCELEROMETER_RAW, 500)
+    imu.enableIMUSensor(dai.IMUSensor.ACCELEROMETER_RAW, 480)
     # enable GYROSCOPE_RAW at 400 hz rate
     imu.enableIMUSensor(dai.IMUSensor.GYROSCOPE_RAW, 400)
     # it's recommended to set both setBatchReportThreshold and setMaxBatchReports to 20 when integrating in a pipeline with a lot of input/output connections
@@ -27,23 +27,20 @@ def timeDeltaToMilliS(delta) -> float:
 
     while pipeline.isRunning():
         imuData = imuQueue.get()
-
+        assert isinstance(imuData, dai.IMUData)
         imuPackets = imuData.packets
         for imuPacket in imuPackets:
             acceleroValues = imuPacket.acceleroMeter
             gyroValues = imuPacket.gyroscope
 
-            acceleroTs = acceleroValues.getTimestampDevice()
-            gyroTs = gyroValues.getTimestampDevice()
-            if baseTs is None:
-                baseTs = acceleroTs if acceleroTs < gyroTs else gyroTs
-            acceleroTs = timeDeltaToMilliS(acceleroTs - baseTs)
-            gyroTs = timeDeltaToMilliS(gyroTs - baseTs)
+            acceleroTs = acceleroValues.getTimestamp()
+            gyroTs = gyroValues.getTimestamp()
 
             imuF = "{:.06f}"
             tsF  = "{:.03f}"
 
-            print(f"Accelerometer timestamp: {tsF.format(acceleroTs)} ms")
+            print(f"Accelerometer timestamp: {acceleroTs}")
+            print(f"Latency [ms]: {dai.Clock.now() - acceleroValues.getTimestamp()}")
             print(f"Accelerometer [m/s^2]: x: {imuF.format(acceleroValues.x)} y: {imuF.format(acceleroValues.y)} z: {imuF.format(acceleroValues.z)}")
-            print(f"Gyroscope timestamp: {tsF.format(gyroTs)} ms")
+            print(f"Gyroscope timestamp: {gyroTs}")
             print(f"Gyroscope [rad/s]: x: {imuF.format(gyroValues.x)} y: {imuF.format(gyroValues.y)} z: {imuF.format(gyroValues.z)} ")
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
@@ -404,3 +404,7 @@ dai_set_test_labels(image_manip_v2_node_test ondevice rvc2_all rvc4 ci)
 # Benchmark tests
 dai_add_test(benchmark_test src/ondevice_tests/pipeline/node/benchmark_test.cpp)
 dai_set_test_labels(benchmark_test ondevice rvc2_all rvc4 ci)
+
+# IMU tests
+dai_add_test(imu_test src/ondevice_tests/pipeline/node/imu_test.cpp)
+dai_set_test_labels(imu_test ondevice rvc4 ci) # Many RVC2 devices do not have an IMU which supports the whole test suite
diff --git a/tests/src/ondevice_tests/pipeline/node/imu_test.cpp b/tests/src/ondevice_tests/pipeline/node/imu_test.cpp
@@ -0,0 +1,42 @@
+#include <catch2/catch_all.hpp>
+#include <catch2/catch_test_macros.hpp>
+#include <initializer_list>
+#include "depthai/depthai.hpp"
+#include "depthai/properties/IMUProperties.hpp"
+
+
+void basicIMUTest(float fps, std::initializer_list<dai::IMUSensor> sensors) {
+    dai::Pipeline p;
+    auto imu = p.create<dai::node::IMU>();
+    for(auto sensor : sensors) {
+        imu->enableIMUSensor(sensor, fps);
+    }
+    auto benchmarkIn = p.create<dai::node::BenchmarkIn>();
+    imu->out.link(benchmarkIn->input);
+    auto reportQueue = benchmarkIn->report.createOutputQueue();
+    p.start();
+    for(int i = 0; i < 10; i++) {
+        auto reportData = reportQueue->get<dai::BenchmarkReport>();
+        REQUIRE(reportData != nullptr);
+        REQUIRE(reportData->numMessagesReceived > 1);
+        REQUIRE(reportData->fps == Catch::Approx(fps).epsilon(0.3));
+        REQUIRE(reportData->averageLatency > 0.0);
+        REQUIRE(reportData->averageLatency < 1.0);  // Sanity check that the latency measurement works correctly
+    }
+}
+
+TEST_CASE("Test IMU, 30Hz, accelerometer, gyroscope") {
+    basicIMUTest(30.0f, {dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW});
+}
+
+TEST_CASE("Test IMU, 100Hz, accelerometer, gyroscope") {
+    basicIMUTest(100.0f, {dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW});
+}
+
+TEST_CASE("Test IMU, all sensors") {
+    basicIMUTest(50.0f, {dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW, dai::IMUSensor::MAGNETOMETER_RAW, dai::IMUSensor::ROTATION_VECTOR});
+}
+
+TEST_CASE("Test IMU, gyroscope 480 Hz") {
+    basicIMUTest(480.0f, {dai::IMUSensor::GYROSCOPE_RAW});
+}