Imu sync fix noetic (#358)

luxonis · Aug 7, 2023 · 1454cb6 · 1454cb6
1 parent a5e7b64
commit 1454cb6
Show file tree

Hide file tree

Showing 30 changed files with 153 additions and 76 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/ImuConverter.hpp b/depthai_bridge/include/depthai_bridge/ImuConverter.hpp
@@ -109,14 +109,22 @@ class ImuConverter {
                 if(accelHist.size() < 3) {
                     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) {
                     continue;
                 } else {
-                    interpolate(gyroHist, accelHist, rotationHist, magnHist, imuMsgs);
+                    if(_enable_rotation) {
+                        interpolate(gyroHist, accelHist, rotationHist, magnHist, imuMsgs);
+                    } else {
+                        interpolate(gyroHist, accelHist, imuMsgs);
+                    }
                 }
             }
         }
@@ -157,6 +165,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="[email protected]">Sachin Guruswamy</maintainer>

diff --git a/depthai_bridge/src/ImuConverter.cpp b/depthai_bridge/src/ImuConverter.cpp
@@ -97,16 +97,14 @@ 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);
         }
     }
@@ -122,16 +120,19 @@ void ImuConverter::toRosDaiMsg(std::shared_ptr<dai::IMUData> inData, std::deque<
         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;
             depthai_ros_msgs::ImuWithMagneticField 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);
+            if(_enable_rotation) {
+                auto rot = inData->packets[i].rotationVector;
+                auto magn = inData->packets[i].magneticField;
+                CreateUnitMessage(accel, gyro, rot, magn, msg, tstamp);
+            } else {
+                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.10.2)
-project(depthai_descriptions VERSION 2.7.4 LANGUAGES CXX C)
+project(depthai_descriptions VERSION 2.7.5 LANGUAGES CXX C)
 
 
 find_package(catkin REQUIRED

diff --git a/depthai_descriptions/package.xml b/depthai_descriptions/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>depthai_descriptions</name>
-  <version>2.7.4</version>
+  <version>2.7.5</version>
   <description>The depthai_descriptions package</description>
 
   <maintainer email="[email protected]">Adam Serafin</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
@@ -100,8 +100,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);
 
@@ -137,7 +137,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::Image, dai::ImgFrame> depthPublish(depthQueue,
                                                                                     pnh,

diff --git a/depthai_examples/src/rgb_stereo_node.cpp b/depthai_examples/src/rgb_stereo_node.cpp
@@ -48,9 +48,9 @@ dai::Pipeline createPipeline(bool lrcheck, bool extended, bool subpixel, int con
 
     // 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);
@@ -60,7 +60,7 @@ dai::Pipeline createPipeline(bool lrcheck, bool extended, bool subpixel, int con
     stereo->setLeftRightCheck(lrcheck);
     stereo->setExtendedDisparity(extended);
     stereo->setSubpixel(subpixel);
-    // stereo->setDepthAlign(dai::CameraBoardSocket::RGB);
+    // stereo->setDepthAlign(dai::CameraBoardSocket::CAM_A);
     // // Link plugins CAM -> STEREO -> XLINK
     monoLeft->out.link(stereo->left);
     monoRight->out.link(stereo->right);
@@ -70,7 +70,7 @@ dai::Pipeline createPipeline(bool lrcheck, bool extended, bool subpixel, int con
     auto colorCam = pipeline.create<dai::node::ColorCamera>();
     auto xlinkOut = pipeline.create<dai::node::XLinkOut>();
     xlinkOut->setStreamName("video");
-    colorCam->setBoardSocket(dai::CameraBoardSocket::RGB);
+    colorCam->setBoardSocket(dai::CameraBoardSocket::CAM_A);
 
     colorCam->setResolution(dai::ColorCameraProperties::SensorResolution::THE_1080_P);
     colorCam->setInterleaved(true);
@@ -123,7 +123,7 @@ int main(int argc, char** argv) {
     std::string color_uri = camera_param_uri + "/" + "color.yaml";
 
     dai::rosBridge::ImageConverter depthConverter(tfPrefix + "_right_camera_optical_frame", true);
-    auto rgbCameraInfo = depthConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::RGB, 1280, 720);
+    auto rgbCameraInfo = depthConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, 1280, 720);
 
     dai::rosBridge::BridgePublisher<sensor_msgs::Image, dai::ImgFrame> depthPublish(stereoQueue,
                                                                                     pnh,

diff --git a/depthai_examples/src/stereo_inertial_publisher.cpp b/depthai_examples/src/stereo_inertial_publisher.cpp
@@ -97,10 +97,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
@@ -110,7 +110,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);
@@ -123,7 +123,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);
         camRgb->setFps(stereo_fps);
         dai::node::ColorCamera::Properties::SensorResolution rgbResolution;
 
@@ -494,10 +494,10 @@ 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::Image, dai::ImgFrame> depthPublish(
@@ -515,7 +515,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::Image, dai::ImgFrame> rgbPublish(
                 imgQueue,
@@ -530,7 +530,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::Image, dai::ImgFrame> previewPublish(
                     previewQueue,
@@ -555,8 +555,8 @@ int main(int argc, char** argv) {
 
             ros::spin();
         } 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);
@@ -584,9 +584,9 @@ int main(int argc, char** argv) {
         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, width, height) : rightCameraInfo;
+            depth_aligned ? rgbConverter.calibrationToCameraInfo(calibrationHandler, dai::CameraBoardSocket::CAM_A, width, height) : rightCameraInfo;
         auto depthconverter = depth_aligned ? rgbConverter : rightconverter;
         dai::rosBridge::BridgePublisher<stereo_msgs::DisparityImage, dai::ImgFrame> dispPublish(
             stereoQueue,
@@ -599,7 +599,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::Image, dai::ImgFrame> rgbPublish(
@@ -615,7 +615,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::Image, dai::ImgFrame> previewPublish(
                     previewQueue,
@@ -640,8 +640,8 @@ int main(int argc, char** argv) {
 
             ros::spin();
         } 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);