From b8269c5a4301ce1f34914652447ac9f5f0e9ce98 Mon Sep 17 00:00:00 2001 From: Szymon Lis Date: Wed, 24 Jan 2024 12:12:09 +0100 Subject: [PATCH 1/6] Added thread-safe timescale provider --- .../Clock/Scripts/TimeScaleProvider.cs | 44 +++++++++++++++++++ .../Clock/Scripts/TimeScaleProvider.cs.meta | 11 +++++ Assets/AWSIM/Scripts/UI/DemoUI.cs | 5 ++- 3 files changed, 59 insertions(+), 1 deletion(-) create mode 100644 Assets/AWSIM/Scripts/Clock/Scripts/TimeScaleProvider.cs create mode 100644 Assets/AWSIM/Scripts/Clock/Scripts/TimeScaleProvider.cs.meta diff --git a/Assets/AWSIM/Scripts/Clock/Scripts/TimeScaleProvider.cs b/Assets/AWSIM/Scripts/Clock/Scripts/TimeScaleProvider.cs new file mode 100644 index 000000000..0306d4cda --- /dev/null +++ b/Assets/AWSIM/Scripts/Clock/Scripts/TimeScaleProvider.cs @@ -0,0 +1,44 @@ +using UnityEngine; + +namespace AWSIM +{ + /// + /// A thread-safe static class to provide the value of the time scale of the simulation. + /// + public static class TimeScaleProvider + { + private static readonly object lockObject = new object(); + + private static float timeScale = 1.0f; + public static float TimeScale + { + get + { + lock (lockObject) + { + return timeScale; + } + } + } + + + #region [Public Methods] + + /// + /// Synchronise the value of the timeScale variable with the Time.timeScale from the Unity thread. + /// + public static void DoUpdate() + { + lock(lockObject) + { + if (Mathf.Abs(Time.timeScale - timeScale) > 0.01f) + { + timeScale = Time.timeScale; + } + } + } + + #endregion + } +} + diff --git a/Assets/AWSIM/Scripts/Clock/Scripts/TimeScaleProvider.cs.meta b/Assets/AWSIM/Scripts/Clock/Scripts/TimeScaleProvider.cs.meta new file mode 100644 index 000000000..0c498dc56 --- /dev/null +++ b/Assets/AWSIM/Scripts/Clock/Scripts/TimeScaleProvider.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 5667e9dc2670888f192424dbcc9f1ade +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/AWSIM/Scripts/UI/DemoUI.cs b/Assets/AWSIM/Scripts/UI/DemoUI.cs index 065491bac..cea278c33 100644 --- a/Assets/AWSIM/Scripts/UI/DemoUI.cs +++ b/Assets/AWSIM/Scripts/UI/DemoUI.cs @@ -24,6 +24,9 @@ public void SetTimeScale(float timeScale) { Time.timeScale = timeScale; timeScaleText.text = "x " + timeScale.ToString("F2"); + + // synchronisation of new timescale value with TimeScaleProvider + TimeScaleProvider.DoUpdate(); } } -} \ No newline at end of file +} From 9bd316e0d53c68a9415abc06d8cd37a27d49040e Mon Sep 17 00:00:00 2001 From: Szymon Lis Date: Wed, 24 Jan 2024 13:42:15 +0100 Subject: [PATCH 2/6] Made clock publish in dedicated thread --- .../Clock/Scripts/DotNetSystemTimeSource.cs | 38 ++++++++++++++++ .../Scripts/DotNetSystemTimeSource.cs.meta | 11 +++++ .../Clock/Scripts/TimeSourceProvider.cs | 15 ++++++- Assets/AWSIM/Scripts/ROS/ClockPublisher.cs | 45 +++++++++++++------ 4 files changed, 95 insertions(+), 14 deletions(-) create mode 100644 Assets/AWSIM/Scripts/Clock/Scripts/DotNetSystemTimeSource.cs create mode 100644 Assets/AWSIM/Scripts/Clock/Scripts/DotNetSystemTimeSource.cs.meta diff --git a/Assets/AWSIM/Scripts/Clock/Scripts/DotNetSystemTimeSource.cs b/Assets/AWSIM/Scripts/Clock/Scripts/DotNetSystemTimeSource.cs new file mode 100644 index 000000000..72011e296 --- /dev/null +++ b/Assets/AWSIM/Scripts/Clock/Scripts/DotNetSystemTimeSource.cs @@ -0,0 +1,38 @@ +using System.Collections; +using System.Collections.Generic; +using UnityEngine; +using System; +using System.Threading; +using ROS2; + +namespace AWSIM +{ + public class DotNetSystemTimeSource : ITimeSource + { + private DateTime prevDateTime; + private double time; + + private static readonly object lockObject = new object(); + + + public DotNetSystemTimeSource() + { + prevDateTime = DateTime.Now; + time = 0.0; + } + + public void GetTime(out int seconds, out uint nanoseconds) + { + lock (lockObject) + { + TimeSpan timeSpan = DateTime.Now - prevDateTime; + prevDateTime = DateTime.Now; + + time += timeSpan.TotalMilliseconds * 0.001f * TimeScaleProvider.TimeScale; + TimeUtils.TimeFromTotalSeconds(time, out seconds, out nanoseconds); + } + + } + } + +} diff --git a/Assets/AWSIM/Scripts/Clock/Scripts/DotNetSystemTimeSource.cs.meta b/Assets/AWSIM/Scripts/Clock/Scripts/DotNetSystemTimeSource.cs.meta new file mode 100644 index 000000000..2abfb4e1b --- /dev/null +++ b/Assets/AWSIM/Scripts/Clock/Scripts/DotNetSystemTimeSource.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 4c8e75c00addd9ed8a48e47d95f9f501 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/AWSIM/Scripts/Clock/Scripts/TimeSourceProvider.cs b/Assets/AWSIM/Scripts/Clock/Scripts/TimeSourceProvider.cs index 63750364e..8b8b22844 100644 --- a/Assets/AWSIM/Scripts/Clock/Scripts/TimeSourceProvider.cs +++ b/Assets/AWSIM/Scripts/Clock/Scripts/TimeSourceProvider.cs @@ -11,7 +11,8 @@ public static class TimeSourceProvider public enum TimeSourceType { UNITY, - SS2 + SS2, + DOTNET } #region [Event] @@ -94,6 +95,18 @@ public static void SetTimeSource(TimeSourceType type) return; } + // dot net system time source + if(type == TimeSourceType.DOTNET) + { + if(currentTimeSource == null || !(currentTimeSource is DotNetSystemTimeSource)) + { + currentTimeSource = new DotNetSystemTimeSource(); + onTimeSourceChanged?.Invoke(); + } + + return; + } + // default time source if(currentTimeSource == null || !(currentTimeSource is UnityTimeSource)) { diff --git a/Assets/AWSIM/Scripts/ROS/ClockPublisher.cs b/Assets/AWSIM/Scripts/ROS/ClockPublisher.cs index f4898318a..33bc224a0 100644 --- a/Assets/AWSIM/Scripts/ROS/ClockPublisher.cs +++ b/Assets/AWSIM/Scripts/ROS/ClockPublisher.cs @@ -1,7 +1,6 @@ -using System.Collections; -using System.Collections.Generic; using UnityEngine; using ROS2; +using System.Threading; namespace AWSIM { @@ -16,7 +15,9 @@ public class ClockPublisher : MonoBehaviour IPublisher clockPublisher; rosgraph_msgs.msg.Clock clockMsg; - float timeScale = 1.0f; + + Thread clockThread; + bool isRunning = false; #region [Life Cycle] @@ -28,32 +29,48 @@ void Awake() clockMsg = new rosgraph_msgs.msg.Clock(); } + void Start() + { + TimeScaleProvider.DoUpdate(); + StartClockThread(); + } + void OnDestroy() { + StopClockThread(); SimulatorROS2Node.RemovePublisher(clockPublisher); } #endregion - void Start() + #region [Clock Thread] + + void StartClockThread() { - timeScale = Time.timeScale; - InvokeRepeating("PublishClock", 1.0f, timeScale/publishHz); + clockThread = new Thread(UpdateClock) + { + Name = "Clock" + }; + isRunning = true; + clockThread.Start(); } - void Update() + void StopClockThread() { - if (Mathf.Abs(Time.timeScale - timeScale) > 0.01f) + isRunning = false; + if (clockThread != null && clockThread.IsAlive) { - timeScale = Time.timeScale; - OnTimeScaleChanged(); + clockThread.Join(); } } - void OnTimeScaleChanged() + void UpdateClock() { - CancelInvoke(); - InvokeRepeating("PublishClock", 0.0f, timeScale/publishHz); + while(isRunning) + { + Thread.Sleep(1000 / publishHz); + PublishClock(); + } } void PublishClock() @@ -61,5 +78,7 @@ void PublishClock() SimulatorROS2Node.UpdateROSClockTime(clockMsg.Clock_); clockPublisher.Publish(clockMsg); } + + #endregion } } From 4ff0f4c050162ad5e8f5f90e5f64e163697e941d Mon Sep 17 00:00:00 2001 From: Szymon Lis Date: Wed, 24 Jan 2024 13:58:16 +0100 Subject: [PATCH 3/6] Added TimeSourceSelector into scene --- .../Scenes/Main/AutowareSimulation.unity | 71 ++++++++++++++++++- 1 file changed, 69 insertions(+), 2 deletions(-) diff --git a/Assets/AWSIM/Scenes/Main/AutowareSimulation.unity b/Assets/AWSIM/Scenes/Main/AutowareSimulation.unity index 2242cfdab..6323ca373 100644 --- a/Assets/AWSIM/Scenes/Main/AutowareSimulation.unity +++ b/Assets/AWSIM/Scenes/Main/AutowareSimulation.unity @@ -38,7 +38,7 @@ RenderSettings: m_ReflectionIntensity: 1 m_CustomReflection: {fileID: 0} m_Sun: {fileID: 0} - m_IndirectSpecularColor: {r: 1537.125, g: 1904.5333, b: 2520.709, a: 1} + m_IndirectSpecularColor: {r: 1520.0924, g: 1884.674, b: 2491.8425, a: 1} m_UseRadianceAmbientProbe: 0 --- !u!157 &3 LightmapSettings: @@ -241,7 +241,8 @@ Transform: m_LocalRotation: {x: -0, y: -0, z: -0, w: 1} m_LocalPosition: {x: 0, y: 0, z: 0} m_LocalScale: {x: 1, y: 1, z: 1} - m_Children: [] + m_Children: + - {fileID: 1575276245} m_Father: {fileID: 543989980} m_RootOrder: 0 m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} @@ -3621,6 +3622,72 @@ MonoBehaviour: timeScaleText: {fileID: 631930841} timeScaleSlider: {fileID: 1153401690} versionText: {fileID: 988705868} +--- !u!1001 &1575276244 +PrefabInstance: + m_ObjectHideFlags: 0 + serializedVersion: 2 + m_Modification: + m_TransformParent: {fileID: 26523764} + m_Modifications: + - target: {fileID: 2426471684872746624, guid: 27181f17b3e1bb607a4b8fcb8a827e0f, type: 3} + propertyPath: m_Name + value: TimeSourceSelector + objectReference: {fileID: 0} + - target: {fileID: 2426471684872746626, guid: 27181f17b3e1bb607a4b8fcb8a827e0f, type: 3} + propertyPath: m_RootOrder + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 2426471684872746626, guid: 27181f17b3e1bb607a4b8fcb8a827e0f, type: 3} + propertyPath: m_LocalPosition.x + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 2426471684872746626, guid: 27181f17b3e1bb607a4b8fcb8a827e0f, type: 3} + propertyPath: m_LocalPosition.y + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 2426471684872746626, guid: 27181f17b3e1bb607a4b8fcb8a827e0f, type: 3} + propertyPath: m_LocalPosition.z + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 2426471684872746626, guid: 27181f17b3e1bb607a4b8fcb8a827e0f, type: 3} + propertyPath: m_LocalRotation.w + value: 1 + objectReference: {fileID: 0} + - target: {fileID: 2426471684872746626, guid: 27181f17b3e1bb607a4b8fcb8a827e0f, type: 3} + propertyPath: m_LocalRotation.x + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 2426471684872746626, guid: 27181f17b3e1bb607a4b8fcb8a827e0f, type: 3} + propertyPath: m_LocalRotation.y + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 2426471684872746626, guid: 27181f17b3e1bb607a4b8fcb8a827e0f, type: 3} + propertyPath: m_LocalRotation.z + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 2426471684872746626, guid: 27181f17b3e1bb607a4b8fcb8a827e0f, type: 3} + propertyPath: m_LocalEulerAnglesHint.x + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 2426471684872746626, guid: 27181f17b3e1bb607a4b8fcb8a827e0f, type: 3} + propertyPath: m_LocalEulerAnglesHint.y + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 2426471684872746626, guid: 27181f17b3e1bb607a4b8fcb8a827e0f, type: 3} + propertyPath: m_LocalEulerAnglesHint.z + value: 0 + objectReference: {fileID: 0} + - target: {fileID: 3077555317673241940, guid: 27181f17b3e1bb607a4b8fcb8a827e0f, type: 3} + propertyPath: Type + value: 2 + objectReference: {fileID: 0} + m_RemovedComponents: [] + m_SourcePrefab: {fileID: 100100000, guid: 27181f17b3e1bb607a4b8fcb8a827e0f, type: 3} +--- !u!4 &1575276245 stripped +Transform: + m_CorrespondingSourceObject: {fileID: 2426471684872746626, guid: 27181f17b3e1bb607a4b8fcb8a827e0f, type: 3} + m_PrefabInstance: {fileID: 1575276244} + m_PrefabAsset: {fileID: 0} --- !u!114 &1672697011 stripped MonoBehaviour: m_CorrespondingSourceObject: {fileID: 9027057599777832477, guid: 58d73df60b244d146bdf5f5896f78355, type: 3} From f7e97732a392199595e0248c71e838788376f10d Mon Sep 17 00:00:00 2001 From: Szymon Lis Date: Wed, 24 Jan 2024 16:13:54 +0100 Subject: [PATCH 4/6] Changed time now to utc now --- .../AWSIM/Scripts/Clock/Scripts/DotNetSystemTimeSource.cs | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/Assets/AWSIM/Scripts/Clock/Scripts/DotNetSystemTimeSource.cs b/Assets/AWSIM/Scripts/Clock/Scripts/DotNetSystemTimeSource.cs index 72011e296..7e41e3b2c 100644 --- a/Assets/AWSIM/Scripts/Clock/Scripts/DotNetSystemTimeSource.cs +++ b/Assets/AWSIM/Scripts/Clock/Scripts/DotNetSystemTimeSource.cs @@ -17,7 +17,7 @@ public class DotNetSystemTimeSource : ITimeSource public DotNetSystemTimeSource() { - prevDateTime = DateTime.Now; + prevDateTime = DateTime.UtcNow; time = 0.0; } @@ -25,8 +25,8 @@ public void GetTime(out int seconds, out uint nanoseconds) { lock (lockObject) { - TimeSpan timeSpan = DateTime.Now - prevDateTime; - prevDateTime = DateTime.Now; + TimeSpan timeSpan = DateTime.UtcNow - prevDateTime; + prevDateTime = DateTime.UtcNow; time += timeSpan.TotalMilliseconds * 0.001f * TimeScaleProvider.TimeScale; TimeUtils.TimeFromTotalSeconds(time, out seconds, out nanoseconds); From 5086832cd8e49f3a6c675d375c4acb4e4a7a86f3 Mon Sep 17 00:00:00 2001 From: Szymon Lis Date: Wed, 24 Jan 2024 16:30:23 +0100 Subject: [PATCH 5/6] Refactor DotNetSystemTimeSource --- .../AWSIM/Scripts/Clock/Scripts/DotNetSystemTimeSource.cs | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/Assets/AWSIM/Scripts/Clock/Scripts/DotNetSystemTimeSource.cs b/Assets/AWSIM/Scripts/Clock/Scripts/DotNetSystemTimeSource.cs index 7e41e3b2c..a9f706f03 100644 --- a/Assets/AWSIM/Scripts/Clock/Scripts/DotNetSystemTimeSource.cs +++ b/Assets/AWSIM/Scripts/Clock/Scripts/DotNetSystemTimeSource.cs @@ -1,6 +1,3 @@ -using System.Collections; -using System.Collections.Generic; -using UnityEngine; using System; using System.Threading; using ROS2; @@ -12,8 +9,7 @@ public class DotNetSystemTimeSource : ITimeSource private DateTime prevDateTime; private double time; - private static readonly object lockObject = new object(); - + private readonly object lockObject = new object(); public DotNetSystemTimeSource() { @@ -31,7 +27,6 @@ public void GetTime(out int seconds, out uint nanoseconds) time += timeSpan.TotalMilliseconds * 0.001f * TimeScaleProvider.TimeScale; TimeUtils.TimeFromTotalSeconds(time, out seconds, out nanoseconds); } - } } From 61f360ed6e6f4a2b86e20a0734b8778818372061 Mon Sep 17 00:00:00 2001 From: Szymon Lis Date: Wed, 24 Jan 2024 16:54:17 +0100 Subject: [PATCH 6/6] Refactor DotNet System TimeSource --- .../AWSIM/Scripts/Clock/Scripts/DotNetSystemTimeSource.cs | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/Assets/AWSIM/Scripts/Clock/Scripts/DotNetSystemTimeSource.cs b/Assets/AWSIM/Scripts/Clock/Scripts/DotNetSystemTimeSource.cs index a9f706f03..40c35c3ac 100644 --- a/Assets/AWSIM/Scripts/Clock/Scripts/DotNetSystemTimeSource.cs +++ b/Assets/AWSIM/Scripts/Clock/Scripts/DotNetSystemTimeSource.cs @@ -4,6 +4,9 @@ namespace AWSIM { + /// + /// A thread-safe timesource class that provides the dot net system utc time. + /// public class DotNetSystemTimeSource : ITimeSource { private DateTime prevDateTime; @@ -21,8 +24,9 @@ public void GetTime(out int seconds, out uint nanoseconds) { lock (lockObject) { - TimeSpan timeSpan = DateTime.UtcNow - prevDateTime; - prevDateTime = DateTime.UtcNow; + DateTime currDateTime = DateTime.UtcNow; + TimeSpan timeSpan = currDateTime - prevDateTime; + prevDateTime = currDateTime; time += timeSpan.TotalMilliseconds * 0.001f * TimeScaleProvider.TimeScale; TimeUtils.TimeFromTotalSeconds(time, out seconds, out nanoseconds);