WIP on time sources

Signed-off-by: Adam Dabrowski <[email protected]>

Author: adamdbrw

src/Ros2ForUnity/Scripts/Time/DotnetTimeSource.cs

@@ -0,0 +1,66 @@
+// Copyright 2022 Robotec.ai.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+using System;
+using System.Threading;
+
+
+namespace ROS2
+{
+
+/// <summary>
+/// DateTime based clock that has resolution increased using Stopwatch.
+/// DateTime is used to synchronize since Stopwatch tends to drift.
+/// </summary>
+public class DotnetTimeSource : ITimeSource
+{
+    private readonly double maxUnsyncedSeconds = 10;
+
+    private Stopwatch stopwatch = new Stopwatch();
+
+    private readonly object mutex = new object();
+
+    private double systemTimeIntervalStart = 0;
+
+    private double stopwatchStartTimeStamp;
+
+    DotnetTimeSource()
+    {
+        stopwatchStartTimeStamp = stopwatch.GetTimestamp();
+    }
+
+    void GetTime(out int seconds, out uint nanoseconds)
+    {
+        lock(mutex) // Threading
+        {
+            double endTimestamp = Stopwatch.GetTimestamp();
+            var durationInSeconds = (endTimestamp - stopwatchStartTimeStamp) / Stopwatch.Frequency;
+            double timeOffset = 0;
+            if (durationInSeconds >= maxUnsyncedSeconds)
+            {   // acquire DateTime to sync
+                stopwatchStartTimeStamp = Stopwatch.GetTimestamp();
+                systemTimeIntervalStart = TimeSpan.FromTicks(DateTime.UtcNow.Ticks).TotalSeconds;
+            }
+            else
+            {   // use Stopwatch offset
+                timeOffset = durationInSeconds;
+            }
+            
+            TimeUtils.TimeFromTotalSeconds(systemTimeIntervalStart + timeOffset, out seconds, out nanoseconds);
+        }
+    }
+}
+
+}  // namespace ROS2
+

src/Ros2ForUnity/Scripts/Time/ITimeSource.cs

@@ -0,0 +1,26 @@
+// Copyright 2022 Robotec.ai.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+namespace ROS2
+{
+
+/// <summary>
+/// Interace for acquiring time
+/// </summary>
+internal interface ITimeSource
+{
+  void GetTime(out int seconds, out uint nanoseconds);
+}
+
+}  // namespace ROS2

src/Ros2ForUnity/Scripts/ROS2Clock.cs renamed to src/Ros2ForUnity/Scripts/Time/ROS2Clock.cs

@@ -1,4 +1,4 @@
-// Copyright 2019-2021 Robotec.ai.
+// Copyright 2019-2022 Robotec.ai.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -19,57 +19,38 @@ namespace ROS2
 {
 
 /// <summary>
-/// A ros2 clock wrapper class that can acquire ros2 time on demand. It does not include a publisher.
-/// (Not every clock needs to be associated with a publisher)
+/// A ros2 clock class that for interfacing between a time source (unity or ros2 system time) and ros2cs messages, structs. 
 /// </summary>
 public class ROS2Clock
 {
-    private int seconds;
-    private uint nanoseconds;
-    private ROS2.Clock clock;
+    private ITimeSource timeSource;
 
-    public ROS2Clock()
-    {
-        clock = new ROS2.Clock();
+    public ROS2Clock() : this(new DotnetTimeSource())
+    {   // By default, use DotnetTimeSource
     }
 
-    ~ROS2Clock()
+    public ROS2Clock(ITimeSource ts)
     {
-        clock.Dispose();
+        timeSource = ts;
     }
 
     public void UpdateClockMessage(ref rosgraph_msgs.msg.Clock clockMessage)
     {
-        GetRosTime();
-        clockMessage.Clock_.Sec = seconds;
-        clockMessage.Clock_.Nanosec = nanoseconds;
+        _timeSource.GetTime(out clockMessage.Clock_.Sec, out clockMessage.Clock_.Nanosec);
     }
 
     public void UpdateROSClockTime(builtin_interfaces.msg.Time time)
     {
-        GetRosTime();
-        time.Nanosec = nanoseconds;
-        time.Sec = seconds;
+        timeSource.GetTime(out time.Sec, out time.Nanosec);
     }
 
     public void UpdateROSTimestamp(ref ROS2.MessageWithHeader message)
     {
-        GetRosTime();
+        int seconds;
+        uint nanoseconds;
+        timeSource.GetTime(out seconds, out nanoseconds);
         message.UpdateHeaderTime(seconds, nanoseconds);
     }
-
-    private void GetRosTime()
-    {
-        if (!ROS2.Ros2cs.Ok())
-        {
-          Debug.LogWarning("Cannot update ros time, ros2 either not initialized or shut down already");
-          return;
-        }
-
-        long nanosec = (long)(clock.Now.Seconds * 1e9);
-        seconds = (int)(nanosec / 1000000000);
-        nanoseconds = (uint)(nanosec % 1000000000);
-    }
 }
 
 }  // namespace ROS2

src/Ros2ForUnity/Scripts/ROS2Clock.cs.meta renamed to src/Ros2ForUnity/Scripts/Time/ROS2Clock.cs.meta

@@ -0,0 +1,54 @@
+// Copyright 2022 Robotec.ai.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+using System;
+
+namespace ROS2
+{
+
+/// <summary>
+/// ros2 time source (system time by default).
+/// </summary>
+public class ROS2TimeSource : ITimeSource
+{
+  private ROS2.Clock clock;
+
+  void GetTime(out int seconds, out uint nanoseconds)
+  {
+    if (!ROS2.Ros2cs.Ok())
+    {
+      seconds = 0;
+      nanoseconds = 0;
+      Debug.LogWarning("Cannot acquire valid ros time, ros either not initialized or shut down already");
+      return;
+    }
+
+    if (clock == null)
+    { // Create clock which uses system time by default (unless use_sim_time is set in ros2)
+      clock = new ROS2.Clock();
+    }
+  
+    TimeUtils.TimeFromTotalSeconds(clock.Now.Seconds, out seconds, out nanoseconds);
+  }
+
+  ~ROS2TimeSource()
+  {
+    if (clock != null)
+    {
+      clock.Dispose();
+    }
+  }
+}
+
+}  // namespace ROS2

src/Ros2ForUnity/Scripts/Time/ROS2TimeSource.cs

@@ -0,0 +1,31 @@
+// Copyright 2022 Robotec.ai.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+namespace ROS2
+{
+
+/// <summary>
+/// Interace for acquiring time
+/// </summary>
+internal static class TimeUtils
+{
+  void TimeFromTotalSeconds(in double seconds, out int seconds, out uint nanoseconds)
+  {
+    long nanosec = (long)(seconds * 1e9);
+    seconds = (int)(nanosec / 1000000000);
+    nanoseconds = (uint)(nanosec % 1000000000);
+  }
+}
+
+}  // namespace ROS2

src/Ros2ForUnity/Scripts/Time/TimeUtils.cs

@@ -0,0 +1,45 @@
+// Copyright 2022 Robotec.ai.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+using System;
+using System.Threading;
+using UnityEngine;
+
+namespace ROS2
+{
+
+/// <summary>
+/// Acquires Unity time. Note that Time API only allows main thread access,
+/// but this class object also stores last acquired value for other threads.
+/// This is done without a warning, so the class will not behave as expected
+/// when not used by main thread.
+/// </summary>
+public class UnityTimeSource : ITimeSource
+{
+  private Thread mainThread;
+  private double lastReadingSecs;
+
+  public UnityTimeSource()
+  {
+    mainThread = Thread.CurrentThread;
+  }
+
+  void GetTime(out int seconds, out uint nanoseconds)
+  {
+    lastReadingSecs = mainThread.Equals(Thread.CurrentThread) ? Time.timeAsDouble : lastReadingSecs;
+    TimeUtils.TimeFromTotalSeconds(lastReadingSecs, out seconds, out nanoseconds);
+  }
+}
+
+}  // namespace ROS2