Iron update 2.7.5 (#367)

luxonis · Aug 11, 2023 · 9c97b34 · 9c97b34
1 parent d2a4438
commit 9c97b34
Show file tree

Hide file tree

Showing 36 changed files with 153 additions and 78 deletions.
diff --git a/depthai-ros/CHANGELOG.rst b/depthai-ros/CHANGELOG.rst
@@ -2,6 +2,10 @@
 Changelog for package depthai-ros
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+2.7.5 (2023-08-07)
+-------------------
+* IMU sync fix
+
 2.7.4 (2023-06-26)
 -------------------
 * ROS time update

diff --git a/depthai-ros/CMakeLists.txt b/depthai-ros/CMakeLists.txt
@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 3.10.2)  # CMake version in Ubuntu 18.04 LTS
 
-project(depthai-ros VERSION 2.7.4 LANGUAGES CXX C)
+project(depthai-ros VERSION 2.7.5 LANGUAGES CXX C)
 
 set(CMAKE_CXX_STANDARD 14)
 

diff --git a/depthai-ros/package.xml b/depthai-ros/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>depthai-ros</name>
-  <version>2.7.4</version>
+  <version>2.7.5</version>
   <description>The depthai-ros package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->

diff --git a/depthai_bridge/CMakeLists.txt b/depthai_bridge/CMakeLists.txt
@@ -1,7 +1,7 @@
 cmake_minimum_required(VERSION 3.10.2)  # CMake version in Ubuntu 18.04 LTS
 set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 
-project(depthai_bridge VERSION 2.7.4 LANGUAGES CXX C)
+project(depthai_bridge VERSION 2.7.5 LANGUAGES CXX C)
 
 set(CMAKE_CXX_STANDARD 14)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)

diff --git a/depthai_bridge/include/depthai_bridge/ImageConverter.hpp b/depthai_bridge/include/depthai_bridge/ImageConverter.hpp
@@ -6,7 +6,7 @@
 #include <tuple>
 #include <unordered_map>
 
-#include "cv_bridge/cv_bridge.h"
+#include "cv_bridge/cv_bridge.hpp"
 #include "depthai-shared/common/CameraBoardSocket.hpp"
 #include "depthai-shared/common/Point2f.hpp"
 #include "depthai/device/CalibrationHandler.hpp"

diff --git a/depthai_bridge/include/depthai_bridge/ImuConverter.hpp b/depthai_bridge/include/depthai_bridge/ImuConverter.hpp
@@ -108,17 +108,25 @@ class ImuConverter {
             }
 
             if(_syncMode == ImuSyncMethod::LINEAR_INTERPOLATE_ACCEL) {
-                if(accelHist.size() < 3) {
+                if(accelHist.size() < 3 && gyroHist.size() && rotationHist.size() && magnHist.size()) {
                     continue;
                 } else {
-                    interpolate(accelHist, gyroHist, rotationHist, magnHist, imuMsgs);
+                    if(_enable_rotation) {
+                        interpolate(accelHist, gyroHist, rotationHist, magnHist, imuMsgs);
+                    } else {
+                        interpolate(accelHist, gyroHist, imuMsgs);
+                    }
                 }
 
             } else if(_syncMode == ImuSyncMethod::LINEAR_INTERPOLATE_GYRO) {
-                if(gyroHist.size() < 3) {
+                if(gyroHist.size() < 3 && accelHist.size() && rotationHist.size() && magnHist.size()) {
                     continue;
                 } else {
-                    interpolate(gyroHist, accelHist, rotationHist, magnHist, imuMsgs);
+                    if(_enable_rotation) {
+                        interpolate(gyroHist, accelHist, rotationHist, magnHist, imuMsgs);
+                    } else {
+                        interpolate(gyroHist, accelHist, imuMsgs);
+                    }
                 }
             }
         }
@@ -159,6 +167,67 @@ class ImuConverter {
         msg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, timestamp);
     }
 
+    template <typename I, typename S, typename M>
+    void CreateUnitMessage(I first, S second, M& msg, std::chrono::_V2::steady_clock::time_point timestamp) {
+        fillImuMsg(first, msg);
+        fillImuMsg(second, msg);
+
+        msg.header.frame_id = _frameName;
+
+        msg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, timestamp);
+    }
+
+    template <typename I, typename S, typename M>
+    void interpolate(std::deque<I>& interpolated, std::deque<S>& second, std::deque<M>& imuMsgs) {
+        I interp0, interp1;
+        S currSecond;
+        interp0.sequence = -1;
+        while(interpolated.size()) {
+            if(interp0.sequence == -1) {
+                interp0 = interpolated.front();
+                interpolated.pop_front();
+            } else {
+                interp1 = interpolated.front();
+                interpolated.pop_front();
+                // remove std::milli to get in seconds
+                std::chrono::duration<double, std::milli> duration_ms = interp1.timestamp.get() - interp0.timestamp.get();
+                double dt = duration_ms.count();
+                while(second.size()) {
+                    currSecond = second.front();
+                    if(currSecond.timestamp.get() > interp0.timestamp.get() && currSecond.timestamp.get() <= interp1.timestamp.get()) {
+                        // remove std::milli to get in seconds
+                        std::chrono::duration<double, std::milli> diff = currSecond.timestamp.get() - interp0.timestamp.get();
+                        const double alpha = diff.count() / dt;
+                        I interp = lerpImu(interp0, interp1, alpha);
+                        M msg;
+                        std::chrono::_V2::steady_clock::time_point tstamp;
+                        if(_getBaseDeviceTimestamp)
+                            tstamp = currSecond.getTimestampDevice();
+                        else
+                            tstamp = currSecond.getTimestamp();
+                        CreateUnitMessage(interp, currSecond, msg, tstamp);
+                        imuMsgs.push_back(msg);
+                        second.pop_front();
+                    } else if(currSecond.timestamp.get() > interp1.timestamp.get()) {
+                        interp0 = interp1;
+                        if(interpolated.size()) {
+                            interp1 = interpolated.front();
+                            interpolated.pop_front();
+                            duration_ms = interp1.timestamp.get() - interp0.timestamp.get();
+                            dt = duration_ms.count();
+                        } else {
+                            break;
+                        }
+                    } else {
+                        second.pop_front();
+                    }
+                }
+                interp0 = interp1;
+            }
+        }
+        interpolated.push_back(interp0);
+    }
+
     template <typename I, typename S, typename T, typename F, typename M>
     void interpolate(std::deque<I>& interpolated, std::deque<S>& second, std::deque<T>& third, std::deque<F>& fourth, std::deque<M>& imuMsgs) {
         I interp0, interp1;

diff --git a/depthai_bridge/package.xml b/depthai_bridge/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>depthai_bridge</name>
-  <version>2.7.4</version>
+  <version>2.7.5</version>
   <description>The depthai_bridge package</description>
 
   <maintainer email="sachin@luxonis.com">Sachin Guruswamy</maintainer>

diff --git a/depthai_bridge/src/ImuConverter.cpp b/depthai_bridge/src/ImuConverter.cpp
@@ -97,16 +97,15 @@ void ImuConverter::toRosMsg(std::shared_ptr<dai::IMUData> inData, std::deque<Imu
         for(int i = 0; i < inData->packets.size(); ++i) {
             auto accel = inData->packets[i].acceleroMeter;
             auto gyro = inData->packets[i].gyroscope;
-            auto rot = inData->packets[i].rotationVector;
-            auto magn = inData->packets[i].magneticField;
             ImuMsgs::Imu msg;
             std::chrono::_V2::steady_clock::time_point tstamp;
             if(_getBaseDeviceTimestamp)
                 tstamp = accel.getTimestampDevice();
             else
                 tstamp = accel.getTimestamp();
 
-            CreateUnitMessage(accel, gyro, rot, magn, msg, tstamp);
+            CreateUnitMessage(accel, gyro, msg, tstamp);
+
             outImuMsgs.push_back(msg);
         }
     }

diff --git a/depthai_descriptions/CMakeLists.txt b/depthai_descriptions/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.8)
-project(depthai_descriptions VERSION 2.7.4)
+project(depthai_descriptions VERSION 2.7.5)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)

diff --git a/depthai_descriptions/package.xml b/depthai_descriptions/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>depthai_descriptions</name>
-  <version>2.7.4</version>
+  <version>2.7.5</version>
   <description>The depthai_descriptions package</description>
 
   <maintainer email="sachin@luxonis.com">Sachin Guruswamy</maintainer>

diff --git a/depthai_examples/CMakeLists.txt b/depthai_examples/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.10.2)  # CMake version in Ubuntu 18.04 LTS
-project(depthai_examples VERSION 2.7.4 LANGUAGES CXX C)
+project(depthai_examples VERSION 2.7.5 LANGUAGES CXX C)
 
 set(CMAKE_CXX_STANDARD 14)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)

diff --git a/depthai_examples/package.xml b/depthai_examples/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>depthai_examples</name>
-  <version>2.7.4</version>
+  <version>2.7.5</version>
   <description>The depthai_examples package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->

diff --git a/depthai_examples/src/depth_crop_control.cpp b/depthai_examples/src/depth_crop_control.cpp
@@ -102,8 +102,8 @@ int main() {
     }
 
     // Properties
-    monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT);
-    monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT);
+    monoRight->setBoardSocket(dai::CameraBoardSocket::CAM_C);
+    monoLeft->setBoardSocket(dai::CameraBoardSocket::CAM_B);
     monoRight->setResolution(monoRes);
     monoLeft->setResolution(monoRes);
 
@@ -139,7 +139,7 @@ int main() {
 
     dai::rosBridge::ImageConverter depthConverter(cameraName + "_right_camera_optical_frame", true);
     // TODO(sachin): Modify the calibration based on crop from service
-    auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, monoWidth, monoHeight);
+    auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_C, monoWidth, monoHeight);
 
     dai::rosBridge::BridgePublisher<sensor_msgs::msg::Image, dai::ImgFrame> depthPublish(
         depthQueue,

diff --git a/depthai_examples/src/rgb_stereo_node.cpp b/depthai_examples/src/rgb_stereo_node.cpp
@@ -70,9 +70,9 @@ std::tuple<dai::Pipeline, int, int> createPipeline(bool lrcheck,
 
     // MonoCamera
     monoLeft->setResolution(monoResolution);
-    monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT);
+    monoLeft->setBoardSocket(dai::CameraBoardSocket::CAM_B);
     monoRight->setResolution(monoResolution);
-    monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT);
+    monoRight->setBoardSocket(dai::CameraBoardSocket::CAM_C);
 
     // StereoDepth
     stereo->initialConfig.setConfidenceThreshold(confidence);
@@ -249,9 +249,9 @@ int main(int argc, char** argv) {
     dai::rosBridge::ImageConverter depthConverter(tfPrefix + "_right_camera_optical_frame", true);
     dai::rosBridge::ImageConverter rgbConverter(tfPrefix + "_rgb_camera_optical_frame", true);
 
-    auto stereoCameraInfo = depthConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, monoWidth, monoHeight);
-    auto previewCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, previewWidth, previewHeight);
-    auto videoCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, colorWidth, colorHeight);
+    auto stereoCameraInfo = depthConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_C, monoWidth, monoHeight);
+    auto previewCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, previewWidth, previewHeight);
+    auto videoCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, colorWidth, colorHeight);
 
     if(useDepth) {
         depthPublish = std::make_unique<dai::rosBridge::BridgePublisher<sensor_msgs::msg::Image, dai::ImgFrame>>(

diff --git a/depthai_examples/src/stereo_inertial_publisher.cpp b/depthai_examples/src/stereo_inertial_publisher.cpp
@@ -96,10 +96,10 @@ std::tuple<dai::Pipeline, int, int> createPipeline(bool enableDepth,
 
     // MonoCamera
     monoLeft->setResolution(monoResolution);
-    monoLeft->setBoardSocket(dai::CameraBoardSocket::LEFT);
+    monoLeft->setBoardSocket(dai::CameraBoardSocket::CAM_B);
     monoLeft->setFps(stereo_fps);
     monoRight->setResolution(monoResolution);
-    monoRight->setBoardSocket(dai::CameraBoardSocket::RIGHT);
+    monoRight->setBoardSocket(dai::CameraBoardSocket::CAM_C);
     monoRight->setFps(stereo_fps);
 
     // StereoDepth
@@ -109,7 +109,7 @@ std::tuple<dai::Pipeline, int, int> createPipeline(bool enableDepth,
     stereo->setLeftRightCheck(lrcheck);
     stereo->setExtendedDisparity(extended);
     stereo->setSubpixel(subpixel);
-    if(enableDepth && depth_aligned) stereo->setDepthAlign(dai::CameraBoardSocket::RGB);
+    if(enableDepth && depth_aligned) stereo->setDepthAlign(dai::CameraBoardSocket::CAM_A);
 
     // Imu
     imu->enableIMUSensor(dai::IMUSensor::ACCELEROMETER_RAW, 500);
@@ -122,7 +122,7 @@ std::tuple<dai::Pipeline, int, int> createPipeline(bool enableDepth,
         auto camRgb = pipeline.create<dai::node::ColorCamera>();
         auto xoutRgb = pipeline.create<dai::node::XLinkOut>();
         xoutRgb->setStreamName("rgb");
-        camRgb->setBoardSocket(dai::CameraBoardSocket::RGB);
+        camRgb->setBoardSocket(dai::CameraBoardSocket::CAM_A);
         dai::node::ColorCamera::Properties::SensorResolution rgbResolution;
 
         if(rgbResolutionStr == "1080p") {
@@ -519,8 +519,8 @@ int main(int argc, char** argv) {
 
     imuPublish.addPublisherCallback();
 
-    // auto leftCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::LEFT, monoWidth, monoHeight);
-    // auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, monoWidth, monoHeight);
+    // auto leftCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_B, monoWidth, monoHeight);
+    // auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_C, monoWidth, monoHeight);
     // const std::string leftPubName = rectify ? std::string("left/image_rect") : std::string("left/image_raw");
     // const std::string rightPubName = rectify ? std::string("right/image_rect") : std::string("right/image_raw");
 
@@ -529,9 +529,9 @@ int main(int argc, char** argv) {
         rgbConverter.setUpdateRosBaseTimeOnToRosMsg();
     }
     if(enableDepth) {
-        auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, width, height);
+        auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_C, width, height);
         auto depthCameraInfo =
-            depth_aligned ? rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, width, height) : rightCameraInfo;
+            depth_aligned ? rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, width, height) : rightCameraInfo;
         auto depthconverter = depth_aligned ? rgbConverter : rightconverter;
 
         dai::rosBridge::BridgePublisher<sensor_msgs::msg::Image, dai::ImgFrame> depthPublish(
@@ -549,7 +549,7 @@ int main(int argc, char** argv) {
         depthPublish.addPublisherCallback();
 
         if(depth_aligned) {
-            auto rgbCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, width, height);
+            auto rgbCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, width, height);
             auto imgQueue = device->getOutputQueue("rgb", 30, false);
             dai::rosBridge::BridgePublisher<sensor_msgs::msg::Image, dai::ImgFrame> rgbPublish(
                 imgQueue,
@@ -564,7 +564,7 @@ int main(int argc, char** argv) {
             if(enableSpatialDetection) {
                 auto previewQueue = device->getOutputQueue("preview", 30, false);
                 auto detectionQueue = device->getOutputQueue("detections", 30, false);
-                auto previewCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, previewWidth, previewHeight);
+                auto previewCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, previewWidth, previewHeight);
 
                 dai::rosBridge::BridgePublisher<sensor_msgs::msg::Image, dai::ImgFrame> previewPublish(
                     previewQueue,
@@ -588,8 +588,8 @@ int main(int argc, char** argv) {
             }
             rclcpp::spin(node);
         } else {
-            auto leftCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::LEFT, width, height);
-            auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, width, height);
+            auto leftCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_B, width, height);
+            auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_C, width, height);
 
             auto leftQueue = device->getOutputQueue("left", 30, false);
             auto rightQueue = device->getOutputQueue("right", 30, false);
@@ -616,10 +616,10 @@ int main(int argc, char** argv) {
     } else {
         std::string tfSuffix = depth_aligned ? "_rgb_camera_optical_frame" : "_right_camera_optical_frame";
         dai::rosBridge::DisparityConverter dispConverter(tfPrefix + tfSuffix, 880, 7.5, 20, 2000);  // TODO(sachin): undo hardcoding of baseline
-        auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, width, height);
+        auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_C, width, height);
 
         auto disparityCameraInfo =
-            depth_aligned ? rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, 1280, 720) : rightCameraInfo;
+            depth_aligned ? rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, 1280, 720) : rightCameraInfo;
         auto depthconverter = depth_aligned ? rgbConverter : rightconverter;
         dai::rosBridge::BridgePublisher<stereo_msgs::msg::DisparityImage, dai::ImgFrame> dispPublish(
             stereoQueue,
@@ -632,7 +632,7 @@ int main(int argc, char** argv) {
         dispPublish.addPublisherCallback();
 
         if(depth_aligned) {
-            auto rgbCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, width, height);
+            auto rgbCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, width, height);
             auto imgQueue = device->getOutputQueue("rgb", 30, false);
             dai::rosBridge::ImageConverter rgbConverter(tfPrefix + "_rgb_camera_optical_frame", false);
             dai::rosBridge::BridgePublisher<sensor_msgs::msg::Image, dai::ImgFrame> rgbPublish(
@@ -647,7 +647,7 @@ int main(int argc, char** argv) {
             if(enableSpatialDetection) {
                 auto previewQueue = device->getOutputQueue("preview", 30, false);
                 auto detectionQueue = device->getOutputQueue("detections", 30, false);
-                auto previewCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, previewWidth, previewHeight);
+                auto previewCameraInfo = rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, previewWidth, previewHeight);
 
                 dai::rosBridge::BridgePublisher<sensor_msgs::msg::Image, dai::ImgFrame> previewPublish(
                     previewQueue,
@@ -671,8 +671,8 @@ int main(int argc, char** argv) {
             }
             rclcpp::spin(node);
         } else {
-            auto leftCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::LEFT, width, height);
-            auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RIGHT, width, height);
+            auto leftCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_B, width, height);
+            auto rightCameraInfo = converter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_C, width, height);
 
             auto leftQueue = device->getOutputQueue("left", 30, false);
             auto rightQueue = device->getOutputQueue("right", 30, false);