Merge pull request #78 from Unity-Technologies/fix-capture-time-math

Fixes math in SimulationState SequenceTimeOfNextCapture

com.unity.perception/Runtime/GroundTruth/SimulationState.cs

 using System.Linq;
 using Newtonsoft.Json.Linq;
 using Unity.Collections;
+using Unity.Mathematics;
 using Unity.Simulation;
 using UnityEngine;
 using UnityEngine.Profiling;
 
         float SequenceTimeOfNextCapture(SensorData sensorData)
         {
+            // If the first capture hasn't happened yet, sequenceTimeNextCapture field won't be valid
-
-            return sensorData.period - (UnscaledSequenceTime - sensorData.firstCaptureTime) % sensorData.period;
+            return sensorData.sequenceTimeNextCapture;
         }
 
         public bool Contains(Guid id) => m_Ids.Contains(id);
                 if (!activeSensor.ShouldCaptureThisFrame)
                     continue;
 
-                //Just in case we get in a situation where we are so far beyond sequenceTimeNextCapture that incrementing next time by the period still doesn't get us to a time past "now"
-                do
-                {
-                    sensorData.sequenceTimeNextCapture += sensorData.period;
-                }
-                while (sensorData.sequenceTimeNextCapture <= UnscaledSequenceTime);
-
+                // TODO: AISV-845 This is an errant modification of this record that can lead to undefined behavior
+                // Leaving as-is for now because too many components depend on this logic
+                sensorData.sequenceTimeNextCapture += sensorData.period;
+                Debug.Assert(sensorData.sequenceTimeNextCapture > UnscaledSequenceTime,
+                    $"Next scheduled capture should be after {UnscaledSequenceTime} but is {sensorData.sequenceTimeNextCapture}");
+                // sensorData.sequenceTimeNextCapture = SequenceTimeOfNextCapture(sensorData);
                 sensorData.lastCaptureFrameCount = Time.frameCount;
                 m_Sensors[activeSensor] = sensorData;
             }

com.unity.perception/Tests/Runtime/GroundTruthTests/DatasetCaptureSensorSchedulingTests.cs

 using System.Collections;
+using System.Collections.Generic;
+using System.Reflection;
 using System.Text.RegularExpressions;
 using NUnit.Framework;
 using UnityEngine;
 namespace GroundTruthTests
 {
+    // Provides accessors and invocation methods for members of SimulationState that would otherwise be in-accessible
+    // due to protection level - use only when testing protected logic is critical
+    class SimulationStateTestHelper
+    {
+        SimulationState m_State => DatasetCapture.SimulationState;
+        Dictionary<SensorHandle, SimulationState.SensorData> m_SensorsReference;
+        MethodInfo m_SequenceTimeOfNextCaptureMethod;
+
+        internal SimulationStateTestHelper()
+        {
+            var bindingFlags = BindingFlags.NonPublic | BindingFlags.Instance;
+            m_SequenceTimeOfNextCaptureMethod = m_State.GetType().GetMethod("SequenceTimeOfNextCapture", bindingFlags);
+            Debug.Assert(m_SequenceTimeOfNextCaptureMethod != null, "Couldn't find sequence time method.");
+            var sensorsField = m_State.GetType().GetField("m_Sensors", bindingFlags);
+            Debug.Assert(sensorsField != null, "Couldn't find internal sensors field");
+            m_SensorsReference = (Dictionary<SensorHandle, SimulationState.SensorData>)(sensorsField.GetValue(m_State));
+            Debug.Assert(m_SensorsReference != null, "Couldn't cast sensor field to dictionary");
+        }
+
+        internal float CallSequenceTimeOfNextCapture(SimulationState.SensorData sensorData)
+        {
+            return (float)m_SequenceTimeOfNextCaptureMethod.Invoke(m_State, new object[] { sensorData });
+        }
+
+        internal SimulationState.SensorData GetSensorData(SensorHandle sensorHandle)
+        {
+            return m_SensorsReference[sensorHandle];
+        }
+    }
+
+        SimulationStateTestHelper m_TestHelper;
+
+        [SetUp]
+        public void SetUp()
+        {
+            m_TestHelper = new SimulationStateTestHelper();
+        }
+
+        }
+
+        [UnityTest]
+        public IEnumerator SequenceTimeOfNextCapture_ReportsCorrectTime()
+        {
+            var ego = DatasetCapture.RegisterEgo("ego");
+            var firstCaptureTime = 1.5f;
+            var period = .4f;
+            var sensorHandle = DatasetCapture.RegisterSensor(ego, "cam", "", period, firstCaptureTime);
+
+            float[] sequenceTimesExpected =
+            {
+                firstCaptureTime,
+                period + firstCaptureTime,
+                period * 2 + firstCaptureTime,
+                period * 3 + firstCaptureTime
+            };
+            for (var i = 0; i < sequenceTimesExpected.Length; i++)
+            {
+                yield return null;
+                var sensorData = m_TestHelper.GetSensorData(sensorHandle);
+                var sequenceTimeActual = m_TestHelper.CallSequenceTimeOfNextCapture(sensorData);
+                Assert.AreEqual(sequenceTimesExpected[i], sequenceTimeActual, 0.0001f);
+            }
         }
 
         [UnityTest]