diff --git a/CMakeLists.txt b/CMakeLists.txt index ea37afb..e6f0387 100644 --- 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.3 LANGUAGES C CXX) include(cmake/Common.cmake) diff --git a/README.md b/README.md index 4d4cd35..2b6f796 100644 --- 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.3-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/2708507/mynt-eye-s-sdk-apidoc-2.2.3-en.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2708508/mynt-eye-s-sdk-apidoc-2.2.3-en.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.3-en/mynt-eye-s-sdk-apidoc-2.2.3-en/index.html) + * zh-Hans: [![](https://img.shields.io/badge/Download-PDF-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2708509/mynt-eye-s-sdk-apidoc-2.2.3-zh-Hans.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK/files/2708510/mynt-eye-s-sdk-apidoc-2.2.3-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.3-zh-Hans/mynt-eye-s-sdk-apidoc-2.2.3-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/2708524/mynt-eye-s-sdk-guide-2.2.3-en.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2708525/mynt-eye-s-sdk-guide-2.2.3-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/2708528/mynt-eye-s-sdk-guide-2.2.3-zh-Hans.pdf) [![](https://img.shields.io/badge/Download-HTML-blue.svg?style=flat)](https://github.com/slightech/MYNT-EYE-S-SDK-Guide/files/2708529/mynt-eye-s-sdk-guide-2.2.3-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.3-zh-Hans/mynt-eye-s-sdk-guide-2.2.3-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][]. The latest version is `2.2.3`. ## Usage diff --git a/doc/en/api.doxyfile b/doc/en/api.doxyfile index 4960085..8775d79 100644 --- 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.3 # 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 diff --git a/doc/zh-Hans/api.doxyfile b/doc/zh-Hans/api.doxyfile index d2b5675..57592e0 100644 --- 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.3 # 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 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 4cce33c..634133c 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 @@ -88,9 +88,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(); @@ -99,7 +100,67 @@ class ROSWrapperNodelet : public nodelet::Nodelet { } return ros::Time( - soft_time_begin + (_hard_time - hard_time_begin) * 0.000001f); + static_cast(soft_time_begin + + static_cast(_hard_time - hard_time_begin) * 0.000001f)); + } + + inline bool is_overflow(std::uint32_t now, + std::uint32_t pre) { + static std::uint64_t unit = + std::numeric_limits::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_annormal(std::uint32_t now, + std::uint32_t pre) { + static std::uint64_t unit = + std::numeric_limits::max(); + + return (now < pre) && ((pre - now) < (unit / 4)); + } + + ros::Time checkUpTimeStamp(std::uint32_t _hard_time, + const Stream &stream) { + static std::map hard_time_now; + static std::map acc; + static std::uint64_t unit_hard_time = + std::numeric_limits::max(); + + if (is_overflow(_hard_time, hard_time_now[stream])) { + acc[stream]++; + } else if (is_repeated(_hard_time, hard_time_now[stream])) { + NODELET_INFO_STREAM("WARNING:: Image time stamp is repeated."); + } else if (is_annormal(_hard_time, hard_time_now[stream])) { + NODELET_INFO_STREAM("WARNING:: Image time stamp is annormal."); + } + 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::max(); + + if (is_overflow(_hard_time, hard_time_now)) { + acc++; + } else if (is_repeated(_hard_time, hard_time_now)) { + NODELET_INFO_STREAM("WARNING:: Imu time stamp is repeated."); + } else if (is_annormal(_hard_time, hard_time_now)) { + NODELET_INFO_STREAM("WARNING:: Imu time stamp is annormal."); + } + hard_time_now = _hard_time; + + return hardTimeToSoftTime( + acc * unit_hard_time + _hard_time); } void onInit() override { @@ -343,7 +404,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); @@ -363,7 +426,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); @@ -378,7 +443,9 @@ class ROSWrapperNodelet : public nodelet::Nodelet { !is_published_[Stream::LEFT]) { api_->SetStreamCallback( Stream::LEFT, [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::LEFT); // static double img_time_prev = -1; // NODELET_INFO_STREAM("ros_time_beg: " << FULL_PRECISION << @@ -409,7 +476,9 @@ class ROSWrapperNodelet : public nodelet::Nodelet { !is_published_[Stream::RIGHT]) { api_->SetStreamCallback( Stream::RIGHT, [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::RIGHT); ++right_count_; publishCamera(Stream::RIGHT, data, right_count_, stamp); @@ -439,7 +508,7 @@ 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 = checkUpImuTimeStamp(data.imu->timestamp); // static double imu_time_prev = -1; // NODELET_INFO_STREAM("ros_time_beg: " << FULL_PRECISION << ros_time_beg @@ -460,7 +529,8 @@ class ROSWrapperNodelet : public nodelet::Nodelet { imu_gyro_ = data.imu; publishImuBySync(stamp); } else { - NODELET_WARN_STREAM("Imu type is unknown"); + publishImu(data, imu_count_, stamp); + publishTemp(data.imu->temperature, imu_count_, stamp); } } else { NODELET_WARN_STREAM("Motion data is empty");