Merge branch 'hotfix/2.2.4'

chore(README.md): remove firmware version info
chore(README.md): update version and doc links
2018-12-31 15:40:27 +08:00 · 2018-12-31 10:33:23 +08:00 · 2018-12-31 10:14:21 +08:00 · 2018-12-31 06:45:25 +08:00 · 2018-12-29 16:47:18 +08:00 · 2018-12-29 16:40:03 +08:00
5 changed files with 91 additions and 21 deletions
--- a/CMakeLists.txt
+++ b/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)

--- a/README.md
+++ b/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/)
-  * 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)
+  * 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/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/)
-  * 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)
+  * 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/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

--- a/doc/en/api.doxyfile
+++ b/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
--- a/doc/zh-Hans/api.doxyfile
+++ b/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
--- a/wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
+++ b/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) {
-            ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
+          Stream::LEFT, [&](const api::StreamData &data) {
+            // 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) {
-            ros::Time stamp = hardTimeToSoftTime(data.img->timestamp);
+          Stream::RIGHT, [&](const api::StreamData &data) {
+            // 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,
Author	SHA1	Message	Date
John Zhao	203c5ce18b	Merge branch 'hotfix/2.2.4'	2018-12-31 15:40:27 +08:00
John Zhao	27ab088acd	chore(README.md): remove firmware version info	2018-12-31 10:33:23 +08:00
John Zhao	2a2ea0b370	chore(README.md): update version and doc links	2018-12-31 10:14:21 +08:00
John Zhao	6172f4c1b8	chore(root): update version	2018-12-31 06:45:25 +08:00
kalman	a3206e5be5	style(ros):rename mutex_data	2018-12-29 16:47:18 +08:00
kalman	e34e6e1466	style(ros):fix spell mistake	2018-12-29 16:40:03 +08:00
kalman	87c3c81a93	fix(ros):delete timestamp repeated/abnormal jundge	2018-12-29 16:32:03 +08:00
TinyOh	ceec4f492a	fix(ros): include publishOther and reduce the range	2018-12-29 09:53:25 +08:00
TinyOh	812638b7dc	fix(ros): record -a core error	2018-12-28 17:49:59 +08:00
kalman	6e638813f2	chore(root):update readme	2018-12-25 19:21:26 +08:00
kalman	18258fe7c9	chore(root):update readme	2018-12-25 19:07:52 +08:00
kalman	0bcaa0801e	fix(README):update url of api doc in English	2018-12-25 18:53:39 +08:00
kalman	b10415515e	chore(root):update reademe	2018-12-25 18:45:48 +08:00
Osenberg	aa19d65272	docs(doc):: Modified version number for release	2018-12-25 11:09:16 +08:00
Osenberg	1f6acd3c98	style(ros):: Remove useless log	2018-12-25 10:35:25 +08:00
Osenberg	28e539e277	fix(ros):: test ros timestamp overflow.	2018-12-23 17:20:03 +08:00
Osenberg	2e97266516	fix(ros): ros time stamp overflow	2018-12-21 20:45:19 +08:00
Osenberg	9eea427d59	Fixed ros timestamp overflow.	2018-12-20 10:58:59 +08:00