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} diff --git a/Assets/AWSIM/Scripts/Clock/Scripts/DotNetSystemTimeSource.cs b/Assets/AWSIM/Scripts/Clock/Scripts/DotNetSystemTimeSource.cs new file mode 100644 index 000000000..40c35c3ac --- /dev/null +++ b/Assets/AWSIM/Scripts/Clock/Scripts/DotNetSystemTimeSource.cs @@ -0,0 +1,37 @@ +using System; +using System.Threading; +using ROS2; + +namespace AWSIM +{ + /// + /// A thread-safe timesource class that provides the dot net system utc time. + /// + public class DotNetSystemTimeSource : ITimeSource + { + private DateTime prevDateTime; + private double time; + + private readonly object lockObject = new object(); + + public DotNetSystemTimeSource() + { + prevDateTime = DateTime.UtcNow; + time = 0.0; + } + + public void GetTime(out int seconds, out uint nanoseconds) + { + lock (lockObject) + { + DateTime currDateTime = DateTime.UtcNow; + TimeSpan timeSpan = currDateTime - prevDateTime; + prevDateTime = currDateTime; + + 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/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/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 } } 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 +}