From 3bdbcdf3933d08abe258f12d7adf2b14ded1488c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 1 Dec 2024 16:14:47 -0800 Subject: [PATCH] Follow-up of previous commit to convert all float64 timestamps correctly --- rtabmap_conversions/src/MsgConversion.cpp | 30 +++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 9753bd887..b41c7ae52 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -3217,6 +3217,21 @@ bool deskew_impl( else if(timeDatatype == 8) //float64 { double sec = *((const double*)(&output.data[u*output.point_step]+offsetTime)); + if(sec > 1.e18) + { + // convert nanoseconds to seconds + sec /= 1.e9; + } + else if(sec > 1.e15) + { + // convert microseconds to seconds + sec /= 1.e6; + } + else if(sec > 1.e12) + { + // sec milliseconds to seconds + sec /= 1.e3; + } stamp = ros::Time(sec); } @@ -3296,6 +3311,21 @@ bool deskew_impl( else if(timeDatatype == 8) { double sec = *((const double*)(&output.data[v*output.row_step]+offsetTime)); + if(sec > 1.e18) + { + // convert nanoseconds to seconds + sec /= 1.e9; + } + else if(sec > 1.e15) + { + // convert microseconds to seconds + sec /= 1.e6; + } + else if(sec > 1.e12) + { + // sec milliseconds to seconds + sec /= 1.e3; + } stamp = ros::Time(sec); }