From cfb8d6f74d0bb6891e74c1254e00d817e5f9ef8c Mon Sep 17 00:00:00 2001
From: Piotr Jaroszek <piotr.jaroszek@robotec.ai>
Date: Mon, 23 Oct 2023 13:29:08 +0200
Subject: [PATCH] Flexible clock (FixedUpdate independent)

Signed-off-by: Piotr Jaroszek <piotr.jaroszek@robotec.ai>
---
 Assets/AWSIM/Scripts/ROS/ClockPublisher.cs | 32 ++++++----------------
 1 file changed, 9 insertions(+), 23 deletions(-)

diff --git a/Assets/AWSIM/Scripts/ROS/ClockPublisher.cs b/Assets/AWSIM/Scripts/ROS/ClockPublisher.cs
index c71d6cd14..ddfa989fe 100644
--- a/Assets/AWSIM/Scripts/ROS/ClockPublisher.cs
+++ b/Assets/AWSIM/Scripts/ROS/ClockPublisher.cs
@@ -12,23 +12,21 @@ public class ClockPublisher : MonoBehaviour
     {
         [SerializeField] string topic;
         [SerializeField] QoSSettings qosSettings;
-        [SerializeField, Range(0, 100)] int publishHz;
+        [SerializeField, Range(1, 100)] int publishHz;
 
         IPublisher<rosgraph_msgs.msg.Clock> clockPublisher;
         rosgraph_msgs.msg.Clock clockMsg;
         float timer = 0;
 
-        void Reset()
+        void PublishClock()
         {
-            topic = "/clock";
-            qosSettings = new QoSSettings()
-            {
-                ReliabilityPolicy = ReliabilityPolicy.QOS_POLICY_RELIABILITY_BEST_EFFORT,
-                DurabilityPolicy = DurabilityPolicy.QOS_POLICY_DURABILITY_VOLATILE,
-                HistoryPolicy = HistoryPolicy.QOS_POLICY_HISTORY_KEEP_LAST,
-                Depth = 1,
-            };
-            publishHz = 100;
+            SimulatorROS2Node.UpdateROSClockTime(clockMsg.Clock_);
+            clockPublisher.Publish(clockMsg);
+        }
+
+        void Start()
+        {
+            InvokeRepeating("PublishClock", 1.0f, 1.0f/publishHz);
         }
 
         // Start is called before the first frame update
@@ -39,18 +37,6 @@ void Awake()
             clockMsg = new rosgraph_msgs.msg.Clock();
         }
 
-        void FixedUpdate()
-        {
-            timer += Time.deltaTime;
-            var interval = 1.0f / publishHz;
-            interval -= 0.00001f;       // Allow for accuracy errors.
-            if (timer < interval)
-                return;
-            timer = 0;
-
-            SimulatorROS2Node.UpdateROSClockTime(clockMsg.Clock_);
-            clockPublisher.Publish(clockMsg);
-        }
 
         void OnDestroy()
         {