.. _program_listing_file__tmp_ws_src_depthai-ros_depthai_bridge_include_depthai_bridge_ImuConverter.hpp:

Program Listing for File ImuConverter.hpp
=========================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_depthai-ros_depthai_bridge_include_depthai_bridge_ImuConverter.hpp>` (``/tmp/ws/src/depthai-ros/depthai_bridge/include/depthai_bridge/ImuConverter.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   #pragma once
   
   #include <deque>
   #include <iostream>
   #include <memory>
   #include <sstream>
   #include <string>
   #include <unordered_map>
   
   #include "depthai-shared/datatype/RawIMUData.hpp"
   #include "depthai/pipeline/datatype/IMUData.hpp"
   #include "depthai_bridge/depthaiUtility.hpp"
   #include "depthai_ros_msgs/msg/imu_with_magnetic_field.hpp"
   #include "rclcpp/time.hpp"
   #include "sensor_msgs/msg/imu.hpp"
   #include "sensor_msgs/msg/magnetic_field.hpp"
   
   namespace dai {
   
   namespace ros {
   
   namespace ImuMsgs = sensor_msgs::msg;
   using ImuPtr = ImuMsgs::Imu::SharedPtr;
   
   enum class ImuSyncMethod { COPY, LINEAR_INTERPOLATE_GYRO, LINEAR_INTERPOLATE_ACCEL };
   
   class ImuConverter {
      public:
       ImuConverter(const std::string& frameName,
                    ImuSyncMethod syncMode = ImuSyncMethod::LINEAR_INTERPOLATE_ACCEL,
                    double linear_accel_cov = 0.0,
                    double angular_velocity_cov = 0.0,
                    double rotation_cov = 0.0,
                    double magnetic_field_cov = 0.0,
                    bool enable_rotation = false,
                    bool enable_magn = false,
                    bool getBaseDeviceTimestamp = false);
       ~ImuConverter();
   
       void updateRosBaseTime();
   
       void setUpdateRosBaseTimeOnToRosMsg(bool update = true) {
           _updateRosBaseTimeOnToRosMsg = update;
       }
   
       void toRosMsg(std::shared_ptr<dai::IMUData> inData, std::deque<ImuMsgs::Imu>& outImuMsgs);
       void toRosDaiMsg(std::shared_ptr<dai::IMUData> inData, std::deque<depthai_ros_msgs::msg::ImuWithMagneticField>& outImuMsgs);
   
       template <typename T>
       T lerp(const T& a, const T& b, const double t) {
           return a * (1.0 - t) + b * t;
       }
   
       template <typename T>
       T lerpImu(const T& a, const T& b, const double t) {
           T res;
           res.x = lerp(a.x, b.x, t);
           res.y = lerp(a.y, b.y, t);
           res.z = lerp(a.z, b.z, t);
           return res;
       }
   
      private:
       template <typename T>
       void FillImuData_LinearInterpolation(std::vector<IMUPacket>& imuPackets, std::deque<T>& imuMsgs) {
           static std::deque<dai::IMUReportAccelerometer> accelHist;
           static std::deque<dai::IMUReportGyroscope> gyroHist;
           static std::deque<dai::IMUReportRotationVectorWAcc> rotationHist;
           static std::deque<dai::IMUReportMagneticField> magnHist;
   
           for(int i = 0; i < imuPackets.size(); ++i) {
               if(accelHist.size() == 0) {
                   accelHist.push_back(imuPackets[i].acceleroMeter);
               } else if(accelHist.back().sequence != imuPackets[i].acceleroMeter.sequence) {
                   accelHist.push_back(imuPackets[i].acceleroMeter);
               }
   
               if(gyroHist.size() == 0) {
                   gyroHist.push_back(imuPackets[i].gyroscope);
               } else if(gyroHist.back().sequence != imuPackets[i].gyroscope.sequence) {
                   gyroHist.push_back(imuPackets[i].gyroscope);
               }
   
               if(_enable_rotation && rotationHist.size() == 0) {
                   rotationHist.push_back(imuPackets[i].rotationVector);
               } else if(_enable_rotation && rotationHist.back().sequence != imuPackets[i].rotationVector.sequence) {
                   rotationHist.push_back(imuPackets[i].rotationVector);
               } else {
                   rotationHist.resize(accelHist.size());
               }
   
               if(_enable_magn && magnHist.size() == 0) {
                   magnHist.push_back(imuPackets[i].magneticField);
               } else if(_enable_magn && magnHist.back().sequence != imuPackets[i].magneticField.sequence) {
                   magnHist.push_back(imuPackets[i].magneticField);
               } else {
                   magnHist.resize(accelHist.size());
               }
   
               if(_syncMode == ImuSyncMethod::LINEAR_INTERPOLATE_ACCEL) {
                   if(accelHist.size() < 3 && gyroHist.size() && rotationHist.size() && magnHist.size()) {
                       continue;
                   } else {
                       if(_enable_rotation) {
                           if(_enable_magn) {
                               interpolate(accelHist, gyroHist, rotationHist, magnHist, imuMsgs);
                           } else {
                               interpolate(accelHist, gyroHist, rotationHist, imuMsgs);
                           }
                       } else {
                           interpolate(accelHist, gyroHist, imuMsgs);
                       }
                   }
   
               } else if(_syncMode == ImuSyncMethod::LINEAR_INTERPOLATE_GYRO) {
                   if(gyroHist.size() < 3 && accelHist.size() && rotationHist.size() && magnHist.size()) {
                       continue;
                   } else {
                       if(_enable_rotation) {
                           if(_enable_magn) {
                               interpolate(gyroHist, accelHist, rotationHist, magnHist, imuMsgs);
                           } else {
                               interpolate(gyroHist, accelHist, rotationHist, imuMsgs);
                           }
                       } else {
                           interpolate(gyroHist, accelHist, imuMsgs);
                       }
                   }
               }
           }
       }
   
       uint32_t _sequenceNum;
       double _linear_accel_cov, _angular_velocity_cov, _rotation_cov, _magnetic_field_cov;
       bool _enable_rotation;
       bool _enable_magn;
       const std::string _frameName = "";
       ImuSyncMethod _syncMode;
       std::chrono::time_point<std::chrono::steady_clock> _steadyBaseTime;
       rclcpp::Time _rosBaseTime;
       bool _getBaseDeviceTimestamp;
       // For handling ROS time shifts and debugging
       int64_t _totalNsChange{0};
       // Whether to update the ROS base time on each message conversion
       bool _updateRosBaseTimeOnToRosMsg{false};
   
       void fillImuMsg(ImuMsgs::Imu& msg, dai::IMUReportAccelerometer report);
       void fillImuMsg(ImuMsgs::Imu& msg, dai::IMUReportGyroscope report);
       void fillImuMsg(ImuMsgs::Imu& msg, dai::IMUReportRotationVectorWAcc report);
       void fillImuMsg(ImuMsgs::Imu& msg, dai::IMUReportMagneticField report);
   
       void fillImuMsg(depthai_ros_msgs::msg::ImuWithMagneticField& msg, dai::IMUReportAccelerometer report);
       void fillImuMsg(depthai_ros_msgs::msg::ImuWithMagneticField& msg, dai::IMUReportGyroscope report);
       void fillImuMsg(depthai_ros_msgs::msg::ImuWithMagneticField& msg, dai::IMUReportRotationVectorWAcc report);
       void fillImuMsg(depthai_ros_msgs::msg::ImuWithMagneticField& msg, dai::IMUReportMagneticField report);
   
       template <typename I, typename S, typename T, typename F, typename M>
       void CreateUnitMessage(M& msg, std::chrono::_V2::steady_clock::time_point timestamp, I first, S second, T third, F fourth) {
           fillImuMsg(msg, first);
           fillImuMsg(msg, second);
           fillImuMsg(msg, third);
           fillImuMsg(msg, fourth);
   
           msg.header.frame_id = _frameName;
   
           msg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, timestamp);
       }
   
       template <typename I, typename S, typename T, typename M>
       void CreateUnitMessage(M& msg, std::chrono::_V2::steady_clock::time_point timestamp, I first, S second, T third) {
           fillImuMsg(msg, first);
           fillImuMsg(msg, second);
           fillImuMsg(msg, third);
   
           msg.header.frame_id = _frameName;
   
           msg.header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, timestamp);
       }
   
       template <typename I, typename S, typename M>
       void CreateUnitMessage(M& msg, std::chrono::_V2::steady_clock::time_point timestamp, I first, S second) {
           fillImuMsg(msg, first);
           fillImuMsg(msg, second);
   
           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(msg, tstamp, interp, currSecond);
                           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 M>
       void interpolate(std::deque<I>& interpolated, std::deque<S>& second, std::deque<T>& third, std::deque<M>& imuMsgs) {
           I interp0, interp1;
           S currSecond;
           T currThird;
           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();
                       currThird = third.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(msg, tstamp, interp, currSecond, currThird);
                           imuMsgs.push_back(msg);
                           second.pop_front();
                           third.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();
                           third.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;
           S currSecond;
           T currThird;
           F currFourth;
           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();
                       currThird = third.front();
                       currFourth = fourth.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(msg, tstamp, interp, currSecond, currThird, currFourth);
                           imuMsgs.push_back(msg);
                           second.pop_front();
                           third.pop_front();
                           fourth.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();
                           third.pop_front();
                           fourth.pop_front();
                       }
                   }
                   interp0 = interp1;
               }
           }
           interpolated.push_back(interp0);
       }
   };
   
   }  // namespace ros
   
   namespace rosBridge = ros;
   
   }  // namespace dai