From 06e8e28b718288fbb5a6217cf5470c4fbe286c85 Mon Sep 17 00:00:00 2001
From: kalman <kalmanxiong@slightech.com>
Date: Mon, 1 Apr 2019 19:54:50 +0800
Subject: [PATCH] fix(ros): fix imu publish bug

---
 .../ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc   | 8 ++++----
 1 file changed, 4 insertions(+), 4 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 6e86dc2..3a1807f 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
@@ -712,10 +712,10 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
           if (data.imu) {
             if (data.imu->flag == 1) {  // accelerometer
               imu_accel_ = data.imu;
-              publishImuBySync(stamp);
+              publishImuBySync();
             } else if (data.imu->flag == 2) {  // gyroscope
               imu_gyro_ = data.imu;
-              publishImuBySync(stamp);
+              publishImuBySync();
             } else {
               publishImu(data, imu_count_, stamp);
               publishTemperature(data.imu->temperature, imu_count_, stamp);
@@ -980,14 +980,14 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     }
   }
 
-  void publishImuBySync(ros::Time stamp) {
+  void publishImuBySync() {
     timestampAlign();
 
     if (imu_accel_ == nullptr || imu_gyro_ == nullptr) {
       return;
     }
     sensor_msgs::Imu msg;
-
+    ros::Time stamp = checkUpImuTimeStamp(imu_accel_->timestamp);
     msg.header.seq = imu_sync_count_;
     msg.header.stamp = stamp;
     msg.header.frame_id = imu_frame_id_;