From aa089a5859099163974edcbbfe43dc7e267fdd60 Mon Sep 17 00:00:00 2001
From: kalman <kalmanxiong@slightech.com>
Date: Fri, 25 Jan 2019 17:33:12 +0800
Subject: [PATCH 1/2] fix(ros): Align imu time stamp

---
 .../src/wrapper_nodelet.cc                    | 68 +++++++++++++++++++
 1 file changed, 68 insertions(+)

diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
index 3f7b7cb..f7fc73d 100644
--- a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
@@ -783,7 +783,75 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     pub_imu_.publish(msg);
   }
 
+  void timestampAlign() {
+    static bool get_first_acc = false;
+    static bool get_first_gyro = false;
+    static bool has_gyro = false;
+    static ImuData acc;
+    static ImuData gyro;
+
+    if (!get_first_acc && imu_accel_ != nullptr) {
+      imu_accel_->flag = 0;
+      acc = *imu_accel_;
+      imu_accel_ = nullptr;
+      get_first_acc = true;
+      return;
+    }
+
+    if (!get_first_gyro && imu_gyro_ != nullptr) {
+      imu_gyro_->flag = 0;
+      gyro = *imu_gyro_;
+      imu_gyro_ = nullptr;
+      get_first_gyro = true;
+      return;
+    }
+
+    if (imu_accel_ != nullptr) {
+      if (!has_gyro) {
+        imu_accel_->flag = 0;
+        acc = *imu_accel_;
+        imu_accel_ = nullptr;
+        return;
+      }
+
+      if (acc.timestamp <= gyro.timestamp) {
+        ImuData acc_temp;
+        acc_temp = *imu_accel_;
+        acc_temp.timestamp = gyro.timestamp;
+
+        double k = static_cast<double>(imu_accel_->timestamp - acc.timestamp);
+        k = static_cast<double>(gyro.timestamp - acc.timestamp) / k;
+
+        acc_temp.accel[0] = acc.accel[0] +
+                            (imu_accel_->accel[0] - acc.accel[0]) * k;
+        acc_temp.accel[1] = acc.accel[1] +
+                            (imu_accel_->accel[1] - acc.accel[1]) * k;
+        acc_temp.accel[2] = acc.accel[2] +
+                            (imu_accel_->accel[2] - acc.accel[2]) * k;
+
+        acc = *imu_accel_;
+        *imu_accel_ = acc_temp;
+        imu_gyro_.reset(new ImuData(gyro));
+        has_gyro = false;
+        return;
+      } else {
+        acc = *imu_accel_;
+        imu_accel_ = nullptr;
+        return;
+      }
+    }
+
+    if (imu_gyro_ != nullptr) {
+      has_gyro = true;
+      gyro = *imu_gyro_;
+      imu_gyro_ = nullptr;
+      return;
+    }
+  }
+
   void publishImuBySync(ros::Time stamp) {
+    timestampAlign();
+
     if (imu_accel_ == nullptr || imu_gyro_ == nullptr) {
       return;
     }

From 52fbb3668da087416cf14d0b9659aefde3011656 Mon Sep 17 00:00:00 2001
From: kalman <kalmanxiong@slightech.com>
Date: Fri, 25 Jan 2019 21:00:12 +0800
Subject: [PATCH 2/2] fix(ros): delete useless code

---
 wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc | 3 ---
 1 file changed, 3 deletions(-)

diff --git a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
index f7fc73d..7cda58a 100644
--- a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
+++ b/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
@@ -791,7 +791,6 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     static ImuData gyro;
 
     if (!get_first_acc && imu_accel_ != nullptr) {
-      imu_accel_->flag = 0;
       acc = *imu_accel_;
       imu_accel_ = nullptr;
       get_first_acc = true;
@@ -799,7 +798,6 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     }
 
     if (!get_first_gyro && imu_gyro_ != nullptr) {
-      imu_gyro_->flag = 0;
       gyro = *imu_gyro_;
       imu_gyro_ = nullptr;
       get_first_gyro = true;
@@ -808,7 +806,6 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
 
     if (imu_accel_ != nullptr) {
       if (!has_gyro) {
-        imu_accel_->flag = 0;
         acc = *imu_accel_;
         imu_accel_ = nullptr;
         return;