added high precision clock and ros header timestamp using high precision clock

Author: micahpearlman

Runtime/Scripts/ROS/MessageTypes/Std/ZOROSStdMessages.cs

@@ -22,12 +22,15 @@ public TimeMessage(uint secs, uint nsecs) {
             this.nsecs = nsecs;
         }
 
+        [Newtonsoft.Json.JsonIgnore]
+        private ZO.Util.ZOClock _clock = new ZO.Util.ZOClock();
         /// <summary>
         /// Sets the timestamp to Now
         /// </summary>
         public void Now() {
             DateTime epoch = new DateTime(1970, 1, 1, 0, 0, 0, 0, DateTimeKind.Utc);
-            TimeSpan timeSpan = DateTime.Now.ToUniversalTime() - epoch;
+            // TimeSpan timeSpan = DateTime.Now.ToUniversalTime() - epoch;
+            TimeSpan timeSpan = _clock.UtcNow - epoch;
             double msecs = timeSpan.TotalMilliseconds;
             secs = (uint)(msecs / 1000);
             nsecs = (uint)((msecs / 1000 - secs) * 1e+9);

Runtime/Scripts/Util/System/ZOClock.cs

@@ -0,0 +1,37 @@
+using System;
+using System.Threading;
+using System.Diagnostics;
+
+namespace ZO.Util {
+    public sealed class ZOClock : IDisposable {
+        private readonly long _maxIdleTime = TimeSpan.FromSeconds(10).Ticks;
+        private const long TicksMultiplier = 1000 * TimeSpan.TicksPerMillisecond;
+
+        private readonly ThreadLocal<DateTime> _startTime =
+            new ThreadLocal<DateTime>(() => DateTime.UtcNow, false);
+
+        private readonly ThreadLocal<double> _startTimestamp =
+            new ThreadLocal<double>(() => Stopwatch.GetTimestamp(), false);
+
+        public DateTime UtcNow {
+            get {
+                double endTimestamp = Stopwatch.GetTimestamp();
+
+                var durationInTicks = (endTimestamp - _startTimestamp.Value) / Stopwatch.Frequency * TicksMultiplier;
+                if (durationInTicks >= _maxIdleTime) {
+                    _startTimestamp.Value = Stopwatch.GetTimestamp();
+                    _startTime.Value = DateTime.UtcNow;
+                    return _startTime.Value;
+                }
+
+                return _startTime.Value.AddTicks((long)durationInTicks);
+            }
+        }
+
+        public void Dispose() {
+            _startTime.Dispose();
+            _startTimestamp.Dispose();
+        }
+    }
+
+}