Merge branch 'hotfix/2.2.4'

CMakeLists.txt

@@ -14,7 +14,7 @@
 
 cmake_minimum_required(VERSION 3.0)
 
-project(mynteye VERSION 2.2.2 LANGUAGES C CXX)
+project(mynteye VERSION 2.2.4 LANGUAGES C CXX)
 
 include(cmake/Common.cmake)
 
@@ -146,7 +146,7 @@ elseif(OS_MAC)
   SET(OSX_EXTRA_LIBS ${VVUVCKIT_LIBRARY} ${USB_LIBRARY})
 
   set(UVC_SRC src/mynteye/uvc/macosx/CameraEngine.cpp src/mynteye/uvc/macosx/AVfoundationCamera.mm src/mynteye/uvc/macosx/uvc-vvuvckit.cc )
-  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -framework CoreFoundation -framework AVFoundation -framework IOKit -framework AppKit -framework Cocoa -framework CoreMedia -framework CoreData -framework Foundation -framework CoreVideo ${__MACUVCLOG_FLAGS}") 
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -framework CoreFoundation -framework AVFoundation -framework IOKit -framework AppKit -framework Cocoa -framework CoreMedia -framework CoreData -framework Foundation -framework CoreVideo ${__MACUVCLOG_FLAGS}")
 
   find_package(libuvc REQUIRED)
   set(UVC_LIB ${libuvc_LIBRARIES})

README.md

@@ -1,6 +1,6 @@
 # MYNT® EYE S SDK
 
-[![](https://img.shields.io/badge/MYNT%20EYE%20S%20SDK-2.2.2-brightgreen.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK)
+[![](https://img.shields.io/badge/MYNT%20EYE%20S%20SDK-2.2.4-brightgreen.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK)
 
 ## Overview
 
@@ -17,11 +17,11 @@ Please follow the guide doc to install the SDK on different platforms.
 ## Documentations
 
 * [API Doc](https://github.com/slightech/MYNT-EYE-S-SDK/releases): API reference, some guides and data spec.
-  * en: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2683636/mynt-eye-s-sdk-apidoc-2.2.2-en.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2683637/mynt-eye-s-sdk-apidoc-2.2.2-en.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](https://slightech.github.io/MYNT-EYE-S-SDK/)
+  * en: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2717670/mynt-eye-s-sdk-apidoc-2.2.4-en.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2717671/mynt-eye-s-sdk-apidoc-2.2.4-en.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](https://slightech.github.io/MYNT-EYE-S-SDK/)
-  * zh-Hans: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2683638/mynt-eye-s-sdk-apidoc-2.2.2-zh-Hans.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2683639/mynt-eye-s-sdk-apidoc-2.2.2-zh-Hans.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](http://doc.myntai.com/resource/api/mynt-eye-s-sdk-apidoc-2.2.2-zh-Hans/mynt-eye-s-sdk-apidoc-2.2.2-zh-Hans/index.html)
+  * zh-Hans: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2717672/mynt-eye-s-sdk-apidoc-2.2.4-zh-Hans.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2717673/mynt-eye-s-sdk-apidoc-2.2.4-zh-Hans.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](http://doc.myntai.com/resource/api/mynt-eye-s-sdk-apidoc-2.2.4-zh-Hans/mynt-eye-s-sdk-apidoc-2.2.4-zh-Hans/index.html)
 * [Guide Doc](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/releases): How to install and start using the SDK.
-  * en: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2683625/mynt-eye-s-sdk-guide-2.2.2-en.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2683626/mynt-eye-s-sdk-guide-2.2.2-en.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](https://slightech.github.io/MYNT-EYE-S-SDK-Guide/)
+  * en: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2717663/mynt-eye-s-sdk-guide-2.2.4-en.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2717664/mynt-eye-s-sdk-guide-2.2.4-en.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](https://slightech.github.io/MYNT-EYE-S-SDK-Guide/)
-  * zh-Hans: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2683627/mynt-eye-s-sdk-guide-2.2.2-zh-Hans.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2683628/mynt-eye-s-sdk-guide-2.2.2-zh-Hans.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](http://doc.myntai.com/resource/sdk/mynt-eye-s-sdk-guide-2.2.2-zh-Hans/mynt-eye-s-sdk-guide-2.2.2-zh-Hans/index.html)
+  * zh-Hans: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2717665/mynt-eye-s-sdk-guide-2.2.4-zh-Hans.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2717666/mynt-eye-s-sdk-guide-2.2.4-zh-Hans.zip) [![](https://img.shields.io/badge/Online-HTML-blue.svg?style=flat)](http://doc.myntai.com/resource/sdk/mynt-eye-s-sdk-guide-2.2.4-zh-Hans/mynt-eye-s-sdk-guide-2.2.4-zh-Hans/index.html)
 
 > Supported languages: `en`, `zh-Hans`.
 
@@ -29,7 +29,7 @@ Please follow the guide doc to install the SDK on different platforms.
 
 [MYNTEYE_BOX]: http://doc.myntai.com/mynteye/s/download
 
-Get firmwares from our online disks: [MYNTEYE_BOX][]. The latest version is `2.2.2`.
+Get firmwares from our online disks: [MYNTEYE_BOX][].
 
 ## Usage
 

doc/en/api.doxyfile

@@ -38,7 +38,7 @@ PROJECT_NAME           = "MYNT EYE S SDK"
 # could be handy for archiving the generated documentation or if some version
 # control system is used.
 
-PROJECT_NUMBER         = 2.2.2
+PROJECT_NUMBER         = 2.2.4
 
 # Using the PROJECT_BRIEF tag one can provide an optional one line description
 # for a project that appears at the top of each page and should give viewer a

doc/zh-Hans/api.doxyfile

@@ -38,7 +38,7 @@ PROJECT_NAME           = "MYNT EYE S SDK"
 # could be handy for archiving the generated documentation or if some version
 # control system is used.
 
-PROJECT_NUMBER         = 2.2.2
+PROJECT_NUMBER         = 2.2.4
 
 # Using the PROJECT_BRIEF tag one can provide an optional one line description
 # for a project that appears at the top of each page and should give viewer a

wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc

@@ -50,7 +50,8 @@ inline double compute_time(const double end, const double start) {
 
 class ROSWrapperNodelet : public nodelet::Nodelet {
  public:
-  ROSWrapperNodelet() {}
+    ROSWrapperNodelet() {
+    }
 
   ~ROSWrapperNodelet() {
     // std::cout << __func__ << std::endl;
@@ -81,9 +82,10 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     }
   }
 
-  ros::Time hardTimeToSoftTime(double _hard_time) {
+  ros::Time hardTimeToSoftTime(std::uint64_t _hard_time) {
     static bool isInited = false;
-    static double soft_time_begin(0), hard_time_begin(0);
+    static double soft_time_begin(0);
+    static std::uint64_t hard_time_begin(0);
 
     if (false == isInited) {
       soft_time_begin = ros::Time::now().toSec();
@@ -92,13 +94,68 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     }
 
     return ros::Time(
-        soft_time_begin + (_hard_time - hard_time_begin) * 0.00001f);
+        static_cast<double>(soft_time_begin +
+        static_cast<double>(_hard_time - hard_time_begin) * 0.00001f));
+  }
+
+  inline bool is_overflow(std::uint32_t now,
+      std::uint32_t pre) {
+    static std::uint64_t unit =
+      std::numeric_limits<std::uint32_t>::max();
+
+    return (now < pre) && ((pre - now) > (unit / 2));
+  }
+
+  inline bool is_repeated(std::uint32_t now,
+      std::uint32_t pre) {
+    return now == pre;
+  }
+
+  inline bool is_abnormal(std::uint32_t now,
+      std::uint32_t pre) {
+    static std::uint64_t unit =
+      std::numeric_limits<std::uint32_t>::max();
+
+    return (now < pre) && ((pre - now) < (unit / 4));
+  }
+
+  ros::Time checkUpTimeStamp(std::uint32_t _hard_time,
+      const Stream &stream) {
+    static std::map<Stream, std::uint32_t> hard_time_now;
+    static std::map<Stream, std::uint32_t> acc;
+    static std::uint64_t unit_hard_time =
+      std::numeric_limits<std::uint32_t>::max();
+
+    if (is_overflow(_hard_time, hard_time_now[stream])) {
+      acc[stream]++;
+    }
+
+    hard_time_now[stream] = _hard_time;
+
+    return hardTimeToSoftTime(
+        acc[stream] * unit_hard_time + _hard_time);
+  }
+
+  ros::Time checkUpImuTimeStamp(std::uint32_t _hard_time) {
+    static std::uint32_t hard_time_now(0), acc(0);
+    static std::uint64_t unit_hard_time =
+      std::numeric_limits<std::uint32_t>::max();
+
+    if (is_overflow(_hard_time, hard_time_now)) {
+      acc++;
+    }
+
+    hard_time_now = _hard_time;
+
+    return hardTimeToSoftTime(
+        acc * unit_hard_time + _hard_time);
   }
 
   void onInit() override {
     initDevice();
     NODELET_FATAL_COND(api_ == nullptr, "No MYNT EYE device selected :(");
 
+    pthread_mutex_init(&mutex_data_, nullptr);
     nh_ = getMTNodeHandle();
     private_nh_ = getMTPrivateNodeHandle();
 
@@ -322,7 +379,9 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
       api_->EnableStreamData(Stream::POINTS);
       api_->SetStreamCallback(
           Stream::POINTS, [this](const api::StreamData &data) {
-            ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
+            // ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
+            ros::Time stamp = checkUpTimeStamp(
+                data.img->timestamp, Stream::POINTS);
             static std::size_t count = 0;
             ++count;
             publishPoints(data, count, stamp);
@@ -342,7 +401,9 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
       api_->EnableStreamData(stream);
       api_->SetStreamCallback(
           stream, [this, stream](const api::StreamData &data) {
-            ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
+            // ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
+            ros::Time stamp = checkUpTimeStamp(
+                data.img->timestamp, stream);
             static std::size_t count = 0;
             ++count;
             publishCamera(stream, data, count, stamp);
@@ -355,8 +416,10 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     if (camera_publishers_[Stream::LEFT].getNumSubscribers() > 0 &&
         !is_published_[Stream::LEFT]) {
       api_->SetStreamCallback(
-          Stream::LEFT, [this](const api::StreamData &data) {
+          Stream::LEFT, [&](const api::StreamData &data) {
-            ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
+            // ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
+            ros::Time stamp = checkUpTimeStamp(
+                data.img->timestamp, Stream::LEFT);
 
             // static double img_time_prev = -1;
             // NODELET_INFO_STREAM("ros_time_beg: " << FULL_PRECISION <<
@@ -384,8 +447,10 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     if (camera_publishers_[Stream::RIGHT].getNumSubscribers() > 0 &&
         !is_published_[Stream::RIGHT]) {
       api_->SetStreamCallback(
-          Stream::RIGHT, [this](const api::StreamData &data) {
+          Stream::RIGHT, [&](const api::StreamData &data) {
-            ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
+            // ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
+            ros::Time stamp = checkUpTimeStamp(
+                data.img->timestamp, Stream::RIGHT);
 
             ++right_count_;
             publishCamera(Stream::RIGHT, data, right_count_, stamp);
@@ -414,7 +479,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
 
     if (!is_motion_published_) {
       api_->SetMotionCallback([this](const api::MotionData &data) {
-        ros::Time stamp = hardTimeToSoftTime(data.imu->timestamp);
+        // ros::Time stamp = hardTimeToSoftTime(data.imu->timestamp);
+        ros::Time stamp = checkUpImuTimeStamp(data.imu->timestamp);
 
         // static double imu_time_prev = -1;
         // NODELET_INFO_STREAM("ros_time_beg: " << FULL_PRECISION <<
@@ -462,12 +528,14 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
     header.seq = seq;
     header.stamp = stamp;
     header.frame_id = frame_ids_[stream];
+    pthread_mutex_lock(&mutex_data_);
     cv::Mat img = data.frame;
     if (stream == Stream::DISPARITY) {  // 32FC1 > 8UC1 = MONO8
       img.convertTo(img, CV_8UC1);
     }
     auto &&msg =
         cv_bridge::CvImage(header, camera_encodings_[stream], img).toImageMsg();
+    pthread_mutex_unlock(&mutex_data_);
     auto &&info = getCameraInfo(stream);
     info->header.stamp = msg->header.stamp;
     camera_publishers_[stream].publish(msg, info);
@@ -902,6 +970,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
   ros::NodeHandle nh_;
   ros::NodeHandle private_nh_;
 
+  pthread_mutex_t mutex_data_;
+
   // camera:
   //   LEFT, RIGHT, LEFT_RECTIFIED, RIGHT_RECTIFIED,
   //   DISPARITY, DISPARITY_NORMALIZED,