tami/MYNT-EYE-S-SDK
6
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed ros timestamp overflow.

Browse Source
This commit is contained in:
Osenberg 2018-12-20 10:58:59 +08:00
parent f6abc57475
commit 9eea427d59
1 changed files with 12 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
wrappers/ros/src/mynt_eye_ros_wrapper/src/wrapper_nodelet.cc
View File

@ -81,9 +81,13 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
} }
} }
ros::Time hardTimeToSoftTime(double _hard_time) { ros::Time hardTimeToSoftTime(std::uint32_t _hard_time) {
static bool isInited = false; static bool isInited = false;
static double soft_time_begin(0), hard_time_begin(0); static uint32_t soft_time_begin(0), hard_time_begin(0);
static uint32_t hard_time_now(0);
static std::uint64_t unit_hard_time =
std::numeric_limits<std::uint32_t>::max();
static std::uint32_t acc(0);
if (false == isInited) { if (false == isInited) {
soft_time_begin = ros::Time::now().toSec(); soft_time_begin = ros::Time::now().toSec();
@ -91,8 +95,13 @@ class ROSWrapperNodelet : public nodelet::Nodelet {
isInited = true; isInited = true;
} }
if (_hard_time < hard_time_now) { acc++; }
hard_time_now = _hard_time;
return ros::Time( return ros::Time(
soft_time_begin + (_hard_time - hard_time_begin) * 0.00001f); soft_time_begin +
static_cast<double>(acc * unit_hard_time +
_hard_time - hard_time_begin) * 0.00001f);
} }
void onInit() override { void onInit() override {