Deskewing: add support for timestamp float64 field in nanoseconds (Li…

…vox)
introlab · Dec 1, 2024 · e5667b2 · e5667b2
1 parent 6f81a8f
commit e5667b2
Showing 1 changed file with 19 additions and 0 deletions.
diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp
@@ -3058,6 +3058,25 @@ bool deskew_impl(
 			}
 		}
 
+		if(secFirst > 1.e18)
+		{
+			// convert nanoseconds to seconds
+			secFirst /= 1.e9;
+			secLast /= 1.e9;
+		}
+		else if(secFirst > 1.e15)
+		{
+			// convert microseconds to seconds
+			secFirst /= 1.e6;
+			secLast /= 1.e6;
+		}
+		else if(secFirst > 1.e12)
+		{
+			// convert milliseconds to seconds
+			secFirst /= 1.e3;
+			secLast /= 1.e3;
+		}
+
 		firstStamp = ros::Time(secFirst);
 		lastStamp = ros::Time(secLast);
 	}