Initial Commit

ClockSensor.cs

@@ -0,0 +1,172 @@
+/**
+ * Copyright (c) 2019-2020 LG Electronics, Inc.
+ *
+ * This software contains code licensed as described in LICENSE.
+ *
+ */
+
+using System;
+using System.Collections.Generic;
+using System.Diagnostics;
+using System.Threading;
+using System.Threading.Tasks;
+using UnityEngine;
+using Simulator.Bridge;
+using Simulator.Bridge.Data;
+using Simulator.Utilities;
+using Simulator.Sensors.UI;
+using System.Collections;
+using Simulator.Analysis;
+
+namespace Simulator.Sensors
+{
+    [SensorType("Clock", new[] { typeof(ClockData) })]
+    public partial class ClockSensor : SensorBase
+    {
+        Queue<Tuple<double, float, Action>> MessageQueue =
+            new Queue<Tuple<double, float, Action>>();
+
+        BridgeInstance Bridge;
+        Publisher<ClockData> Publish;
+
+        bool Destroyed = false;
+        bool IsFirstFixedUpdate = true;
+        double LastTimestamp;
+        ClockData latestData;
+        float realTimeStart;
+
+        ClockData data;
+        private bool Sending = false;
+
+        public override SensorDistributionType DistributionType => SensorDistributionType.LowLoad;
+
+        public override void OnBridgeSetup(BridgeInstance bridge)
+        {
+            Bridge = bridge;
+            Publish = bridge.AddPublisher<ClockData>(Topic);
+        }
+
+        public void Start()
+        {
+            Task.Run(Publisher);
+            realTimeStart = Time.time;
+        }
+
+        void OnDestroy()
+        {
+            Destroyed = true;
+        }
+
+        void Publisher()
+        {
+            var nextPublish = Stopwatch.GetTimestamp();
+
+            while (!Destroyed)
+            {
+                long now = Stopwatch.GetTimestamp();
+                if (now < nextPublish)
+                {
+                    Thread.Sleep(0);
+                    continue;
+                }
+
+                Tuple<double, float, Action> msg = null;
+                lock (MessageQueue)
+                {
+                    if (MessageQueue.Count > 0)
+                    {
+                        msg = MessageQueue.Dequeue();
+                    }
+                }
+
+                if (msg != null)
+                {
+                    if (!Sending) // Drop this message if previous sending has not finished.
+                    {
+                        try
+                        {
+                            Sending = true;
+                            msg.Item3();
+                        }
+                        catch
+                        {
+                            Sending = false;
+                        }
+                    }
+                    nextPublish = now + (long)(Stopwatch.Frequency * msg.Item2);
+                    LastTimestamp = msg.Item1;
+                }
+            }
+        }
+
+        void FixedUpdate()
+        {
+            if (IsFirstFixedUpdate)
+            {
+                if (Bridge != null && Bridge.Status == Status.Connected)
+                {
+                    lock (MessageQueue)
+                    {
+                        MessageQueue.Clear();
+                    }
+                }
+                IsFirstFixedUpdate = false;
+            }
+
+            var time = SimulatorManager.Instance.CurrentTime;
+            if (time < LastTimestamp)
+            {
+                return;
+            }
+
+            var data = new ClockData()
+            {
+                Clock = time,
+            };
+
+            latestData = data;
+
+            if (Bridge != null && Bridge.Status == Status.Connected)
+            {
+                lock (MessageQueue)
+                {
+                    MessageQueue.Enqueue(Tuple.Create(time, Time.fixedDeltaTime,
+                        (Action)(() => Publish(data, () => Sending = false))));
+                }
+            }
+        }
+
+        void Update()
+        {
+            IsFirstFixedUpdate = true;
+        }
+
+        [AnalysisMeasurement(MeasurementType.Duration)]
+        public double SimulationDuration => SimulatorManager.Instance.CurrentTime - SimulatorManager.Instance.SessionStartTime;
+
+        [AnalysisMeasurement(MeasurementType.Duration)]
+        public double RealtimeDuration => Time.time - realTimeStart;
+
+        public override void OnVisualize(Visualizer visualizer)
+        {
+            UnityEngine.Debug.Assert(visualizer != null);
+
+            if (latestData == null)
+            {
+                return;
+            }
+
+            var graphData = new Dictionary<string, object>()
+            {
+                {"Time", latestData.Clock},
+                {"Fixed DeltaTime", Time.fixedDeltaTime}
+            };
+            visualizer.UpdateGraphValues(graphData);
+        }
+
+        public override void OnVisualizeToggle(bool state)
+        {
+            //
+        }
+    }
+}

ClockSensor.prefab

@@ -0,0 +1,48 @@
+%YAML 1.1
+%TAG !u! tag:unity3d.com,2011:
+--- !u!1 &6252413388666032885
+GameObject:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  serializedVersion: 6
+  m_Component:
+  - component: {fileID: 6202804750886854075}
+  - component: {fileID: 7298661354176162290}
+  m_Layer: 0
+  m_Name: ClockSensor
+  m_TagString: Untagged
+  m_Icon: {fileID: 0}
+  m_NavMeshLayer: 0
+  m_StaticEditorFlags: 0
+  m_IsActive: 1
+--- !u!4 &6202804750886854075
+Transform:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 6252413388666032885}
+  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_Father: {fileID: 0}
+  m_RootOrder: 0
+  m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
+--- !u!114 &7298661354176162290
+MonoBehaviour:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 6252413388666032885}
+  m_Enabled: 1
+  m_EditorHideFlags: 0
+  m_Script: {fileID: 11500000, guid: e6d09cbb1a438e1459477f1899bdc310, type: 3}
+  m_Name: 
+  m_EditorClassIdentifier: 
+  Name: 
+  Topic: 
+  Frame: