Final fixes of clocks and dotnet time set to unix epoch

Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai>

Author: adamdbrw

src/Ros2ForUnity/Scripts/ROS2UnityComponent.cs

@@ -45,6 +45,8 @@ public bool Ok()
     {
         lock (mutex)
         {
+            if (ros2forUnity == null)
+                LazyConstruct();
             return (nodes != null && ros2forUnity.Ok());
         }
     }

src/Ros2ForUnity/Scripts/Time/DotnetTimeSource.cs

@@ -34,22 +34,32 @@ public class DotnetTimeSource : ITimeSource
 
     private double stopwatchStartTimeStamp;
 
-    public DotnetTimeSource()
+    private double TotalSystemTimeSeconds()
+    {
+        return TimeSpan.FromTicks(DateTime.UtcNow.Ticks).TotalSeconds;
+    }
+
+    private void UpdateSystemTime()
     {
+        systemTimeIntervalStart = TotalSystemTimeSeconds();
         stopwatchStartTimeStamp = Stopwatch.GetTimestamp();
     }
 
+    public DotnetTimeSource()
+    {
+        UpdateSystemTime();
+    }
+
     public void GetTime(out int seconds, out uint nanoseconds)
     {
         lock(mutex) // Threading
         {
             double endTimestamp = Stopwatch.GetTimestamp();
-            var durationInSeconds = (endTimestamp - stopwatchStartTimeStamp) / Stopwatch.Frequency;
+            var durationInSeconds = endTimestamp - stopwatchStartTimeStamp;
             double timeOffset = 0;
             if (durationInSeconds >= maxUnsyncedSeconds)
             {   // acquire DateTime to sync
-                stopwatchStartTimeStamp = Stopwatch.GetTimestamp();
-                systemTimeIntervalStart = TimeSpan.FromTicks(DateTime.UtcNow.Ticks).TotalSeconds;
+                UpdateSystemTime();
             }
             else
             {   // use Stopwatch offset

src/Ros2ForUnity/Scripts/Time/ROS2Clock.cs

@@ -25,8 +25,8 @@ public class ROS2Clock
 {
     private ITimeSource _timeSource;
 
-    public ROS2Clock() : this(new DotnetTimeSource())
-    {   // By default, use DotnetTimeSource
+    public ROS2Clock() : this(new ROS2TimeSource())
+    {   // By default, use ROS2TimeSource
     }
 
     public ROS2Clock(ITimeSource ts)