Follow-up of previous commit to convert all float64 timestamps correctly

introlab · Dec 2, 2024 · 3bdbcdf · 3bdbcdf
1 parent e5667b2
commit 3bdbcdf
Showing 1 changed file with 30 additions and 0 deletions.
diff --git 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);
 			}