Feature/timesource options

Assets/AWSIM/Scenes/Main/AutowareSimulation.unity

@@ -4046,7 +4046,7 @@ PrefabInstance:
       objectReference: {fileID: 0}
     - target: {fileID: 3077555317673241940, guid: 27181f17b3e1bb607a4b8fcb8a827e0f, type: 3}
       propertyPath: Type
-      value: 2
+      value: 3
       objectReference: {fileID: 0}
     m_RemovedComponents: []
   m_SourcePrefab: {fileID: 100100000, guid: 27181f17b3e1bb607a4b8fcb8a827e0f, type: 3}

Assets/AWSIM/Scenes/Main/AutowareSimulation/AutowareSimulation.cs

@@ -12,6 +12,7 @@ public class AutowareSimulation : MonoBehaviour
     {
         [SerializeField] TrafficManager trafficManager;
         [SerializeField] Transform egoTransform;
+        private TimeSourceSelector timeSourceSelector;
 
         [Header("Player Config")]
         [SerializeField] string commandLineConfigParam = "--json_path";
@@ -33,13 +34,15 @@ public class EgoConfiguration
         public class Configuration
         {
             public float TimeScale;                 // Reflected in Time.timeScale
+            public string TimeSource;               // Reflected in TimeSourceSelector
             public int RandomTrafficSeed;           // Reflected in TrafficManager.seed
             public int MaxVehicleCount;             // Reflected in TrafficManager.maxVehicleCount
             public EgoConfiguration Ego = new EgoConfiguration();
         }
 
         void Awake()
         {
+            CollectComponents();
 
 #if !UNITY_EDITOR
             // initialize
@@ -62,6 +65,19 @@ void Awake()
 
                 var rotation = Quaternion.Euler(config.Ego.EulerAngles);
                 egoTransform.rotation = ROS2Utility.RosToUnityRotation(rotation);
+
+                // set time source
+                timeSourceSelector?.SetType(config.TimeSource);
+            }
+        }
+
+        void CollectComponents()
+        {
+            // get time source selector
+            timeSourceSelector = FindObjectOfType<TimeSourceSelector>();
+            if(timeSourceSelector == null)
+            {
+                Debug.LogWarning("TimeSource: There is no TimeSourceSelector object in the active scene. The default time source will be used.");
             }
         }
     }

Assets/AWSIM/Scenes/Main/AutowareSimulation/config.json

@@ -1,5 +1,6 @@
 {
     "TimeScale": 0.2,
+    "TimeSource": "simulation",
     "RandomTrafficSeed": 33,
     "MaxVehicleCount": 2,
     "Ego": {
@@ -14,4 +15,4 @@
             "z": 35.0
         }
     }
-}
+}

Assets/AWSIM/Scripts/Clock/Scripts/DotNetSimulationTimeSource.cs

@@ -0,0 +1,48 @@
+using System;
+using System.Threading;
+using ROS2;
+
+namespace AWSIM
+{
+    /// <summary>
+    /// A thread-safe timesource class that provides simulation time based on the dot net system utc time.
+    /// This time source takes into account the value of the simulation timescale and
+    /// starts at zero value when the simulation is started.
+    /// </summary>
+    public class DotNetSimulationTimeSource : ITimeSource
+    {
+        private DateTime prevDateTime;
+        private double time;
+        private bool hasStarted = false;
+
+        private readonly object lockObject = new object();
+
+        public DotNetSimulationTimeSource()
+        {
+            hasStarted = false;
+        }
+
+        public void GetTime(out int seconds, out uint nanoseconds)
+        {
+            lock (lockObject)
+            {
+                DateTime currDateTime = DateTime.UtcNow;
+
+                if(!hasStarted)
+                {
+                    hasStarted = true;
+                    time = 0.0;
+
+                    prevDateTime = currDateTime;
+                }
+
+                TimeSpan timeSpan = currDateTime - prevDateTime;
+                prevDateTime = currDateTime;
+
+                time += timeSpan.TotalMilliseconds * 0.001f * TimeScaleProvider.TimeScale;
+                TimeUtils.TimeFromTotalSeconds(time, out seconds, out nanoseconds);
+            }
+        }
+    }
+
+}

Assets/AWSIM/Scripts/Clock/Scripts/DotNetSystemTimeSource.cs

@@ -5,30 +5,43 @@
 namespace AWSIM
 {
     /// <summary>
-    /// A thread-safe timesource class that provides the dot net system utc time.
+    /// A thread-safe timesource class that provides the dot net system utc time since epoch.
+    /// This timesource takes into account the value of the simulation timescale.
     /// </summary>
     public class DotNetSystemTimeSource : ITimeSource
     {
         private DateTime prevDateTime;
         private double time;
+        private bool hasStarted = false;
 
         private readonly object lockObject = new object();
 
         public DotNetSystemTimeSource()
         {
-            prevDateTime = DateTime.UtcNow;
-            time = 0.0;
+            hasStarted = false;
         }
 
         public void GetTime(out int seconds, out uint nanoseconds)
         {
             lock (lockObject)
             {
                 DateTime currDateTime = DateTime.UtcNow;
+
+                if(!hasStarted)
+                {
+                    hasStarted = true;
+
+                    // get the time in millisecond since epoch
+                    long timeOffset = ((DateTimeOffset)currDateTime).ToUnixTimeMilliseconds();
+                    time = (double)timeOffset * 0.001;
+
+                    prevDateTime = currDateTime;
+                }
+
                 TimeSpan timeSpan = currDateTime - prevDateTime;
                 prevDateTime = currDateTime;
 
-                time += timeSpan.TotalMilliseconds * 0.001f * TimeScaleProvider.TimeScale;
+                time += timeSpan.TotalMilliseconds * 0.001 * TimeScaleProvider.TimeScale;
                 TimeUtils.TimeFromTotalSeconds(time, out seconds, out nanoseconds);
             }
         }

Assets/AWSIM/Scripts/Clock/Scripts/TimeAsDoubleProvider.cs

@@ -0,0 +1,41 @@
+using UnityEngine;
+
+namespace AWSIM
+{
+    /// <summary>
+    /// A thread-safe static class to provide the value of the Unity time as double.
+    /// </summary>
+    public static class TimeAsDoubleProvider
+    {
+        private static readonly object lockObject = new object();
+
+        private static double timeAsDouble = 0.0;
+        public static double TimeAsDouble
+        {
+            get
+            {
+                lock(lockObject)
+                {
+                    return timeAsDouble;
+                }
+            }
+        }
+
+
+        #region [Public Methods]
+
+        /// <summary>
+        /// Synchronise the value of the timeAsDouble variable with the Time.timeAsDouble from the Unity thread.
+        /// </summary>
+        public static void DoUpdate()
+        {
+            lock(lockObject)
+            {
+                timeAsDouble = Time.timeAsDouble;
+            }
+        }
+
+        #endregion
+    }
+}
+

Assets/AWSIM/Scripts/Clock/Scripts/TimeScaleProvider.cs

@@ -31,10 +31,7 @@ public static void DoUpdate()
         {
             lock(lockObject)
             {
-                if (Mathf.Abs(Time.timeScale - timeScale) > 0.01f)
-                {
-                    timeScale = Time.timeScale;
-                }
+                timeScale = Time.timeScale;
             }
         }
 

Assets/AWSIM/Scripts/Clock/Scripts/TimeSourceProvider.cs

@@ -1,3 +1,4 @@
+using AWSIM.Samples;
 using ROS2;
 using System;
 
@@ -12,7 +13,9 @@ public enum TimeSourceType
         {
             UNITY,
             SS2,
-            DOTNET
+            DOTNET_SYSTEM,
+            DOTNET_SIMULATION,
+            ROS2,
         }
 
         #region [Event]
@@ -96,7 +99,7 @@ public static void SetTimeSource(TimeSourceType type)
             }
 
             // dot net system time source
-            if(type == TimeSourceType.DOTNET)
+            if(type == TimeSourceType.DOTNET_SYSTEM)
             {
                 if(currentTimeSource == null || !(currentTimeSource is DotNetSystemTimeSource))
                 {
@@ -107,6 +110,30 @@ public static void SetTimeSource(TimeSourceType type)
                 return;
             }
 
+            // dot net simulation time source
+            if(type == TimeSourceType.DOTNET_SIMULATION)
+            {
+                if(currentTimeSource == null || !(currentTimeSource is DotNetSimulationTimeSource))
+                {
+                    currentTimeSource = new DotNetSimulationTimeSource();
+                    onTimeSourceChanged?.Invoke();
+                }
+
+                return;
+            }
+
+            // ros2 time source
+            if(type == TimeSourceType.ROS2)
+            {
+                if(currentTimeSource == null || !(currentTimeSource is ROS2TimeSource))
+                {
+                    currentTimeSource = new ROS2TimeSource();
+                    onTimeSourceChanged?.Invoke();
+                }
+
+                return;
+            }
+
             // default time source
             if(currentTimeSource == null || !(currentTimeSource is UnityTimeSource))
             {

Assets/AWSIM/Scripts/Clock/Scripts/TimeSourceSelector.cs

@@ -20,6 +20,60 @@ private void Awake()
             TimeSourceProvider.SetTimeSource(Type);
         }
 
+        #region [Public Methods]
+
+        /// <summary>
+        /// Set the time source of the desired type.
+        /// </summary>
+        /// <param name="type">Type of requested time source.</param>
+        public void SetType(string type)
+        {
+            if(type == null || type == "")
+            {
+                return;
+            }
+
+            if(string.CompareOrdinal(type.ToLower(), "unity") == 0)
+            {
+                SetType(TimeSourceProvider.TimeSourceType.UNITY);
+            }
+            else if(string.CompareOrdinal(type.ToLower(), "simulation") == 0)
+            {
+                SetType(TimeSourceProvider.TimeSourceType.DOTNET_SIMULATION);
+            }
+            else if(string.CompareOrdinal(type.ToLower(), "system") == 0)
+            {
+                SetType(TimeSourceProvider.TimeSourceType.DOTNET_SYSTEM);
+            }
+            else if(string.CompareOrdinal(type.ToLower(), "ss2") == 0)
+            {
+                SetType(TimeSourceProvider.TimeSourceType.SS2);
+            }
+            else if(string.CompareOrdinal(type.ToLower(), "ros2") == 0)
+            {
+                SetType(TimeSourceProvider.TimeSourceType.ROS2);
+            }
+            else
+            {
+                Debug.LogWarning("TimeSourceSelector: " + type + " is not recognized as a valid time source.");
+            }
+        }
+
+        #endregion
+
+        #region [Private Methods]
+
+        /// <summary>
+        /// Set the time source of the desired type.
+        /// </summary>
+        /// <param name="type">Type of requested time source.</param>
+        private void SetType(TimeSourceProvider.TimeSourceType type)
+        {
+            Type = type;
+            TimeSourceProvider.SetTimeSource(Type);
+        }
+
+        #endregion
     }
 
 }

Assets/AWSIM/Scripts/ROS/ClockPublisher.cs

@@ -32,6 +32,7 @@ void Awake()
         void Start()
         {
             TimeScaleProvider.DoUpdate();
+            TimeAsDoubleProvider.DoUpdate();
             StartClockThread();
         }
 
@@ -43,6 +44,15 @@ void OnDestroy()
 
         #endregion
 
+        #region [Main Thread]
+
+        void Update()
+        {
+            TimeAsDoubleProvider.DoUpdate();
+        }
+
+        #endregion
+
         #region [Clock Thread]
 
         void StartClockThread()

Assets/Ros2ForUnity/Scripts/Time/DotnetTimeSource.cs

@@ -22,6 +22,7 @@ namespace ROS2
 /// DateTime based clock that has resolution increased using Stopwatch.
 /// DateTime is used to synchronize since Stopwatch tends to drift.
 /// </summary>
+[System.Obsolete("This TimeSource is deprecated and will be removed in the future versions. Please use DotNetSystemTimeSource instead.")]
 public class DotnetTimeSource : ITimeSource
 {
     private readonly double maxUnsyncedSeconds = 10;

Assets/Ros2ForUnity/Scripts/Time/ROS2ScalableTimeSource.cs

@@ -21,6 +21,7 @@ namespace ROS2
 /// <summary>
 /// ros2 time source (system time by default).
 /// </summary>
+[System.Obsolete("This TimeSource is deprecated and will be removed in the future versions. Please use DotNetSystemTimeSource instead.")]
 public class ROS2ScalableTimeSource : ITimeSource
 {
   private Thread mainThread;