Added Linear Velocity publishing. Added TimeScale configuration.

Unity/Assets/RosSharpModules/GPSPublisher.cs

@@ -41,10 +41,10 @@ protected override void Start()
 
         private void FixedUpdate()
         {
-            if (Time.realtimeSinceStartup >= previousScanTime + updatePeriod)
+            if (UnityEngine.Time.time >= previousScanTime + updatePeriod)
             {
                 WriteMessage();
-                previousScanTime = Time.realtimeSinceStartup;
+                previousScanTime = UnityEngine.Time.time;
             }
         }
 

Unity/Assets/RosSharpModules/IMUPublisher.cs

@@ -38,10 +38,10 @@ protected override void Start()
 
         private void FixedUpdate()
         {
-            if (Time.realtimeSinceStartup >= previousScanTime + updatePeriod)
+            if (UnityEngine.Time.time >= previousScanTime + updatePeriod)
             {
                 WriteMessage();
-                previousScanTime = Time.realtimeSinceStartup;
+                previousScanTime = UnityEngine.Time.time;
             }
         }
 

Unity/Assets/RosSharpModules/LinearVelocityPublisher.cs

@@ -0,0 +1,54 @@
+using UnityEngine;
+
+namespace RosSharp.RosBridgeClient.MessageTypes.swc_msgs
+{
+    [RequireComponent(typeof(AckermannController))]
+    public class LinearVelocityPublisher : UnityPublisher<Gps>
+    {
+        private Gps message;
+
+        private float previousScanTime = 5;
+
+        private float velocityNoiseStdDev = 0.1f;
+
+        private float updatePeriod = 0.05f;
+        private AckermannController c;
+
+        public bool noNoiseOverride = false;
+
+        protected override void Start()
+        {
+            base.Start();
+            c = GetComponent<AckermannController>();
+            message = new Gps();
+
+            switch (ConfigLoader.competition.NoiseLevel) {
+                case ConfigLoader.CompetitionConfig.NoiseLevels.none:
+                    velocityNoiseStdDev *= 0;
+                    break;
+                case ConfigLoader.CompetitionConfig.NoiseLevels.reduced:
+                    velocityNoiseStdDev *= 0.5f;
+                    break;
+            }
+
+            if (noNoiseOverride) {
+                velocityNoiseStdDev = 0;
+            }
+        }
+
+        private void FixedUpdate()
+        {
+            if (UnityEngine.Time.time >= previousScanTime + updatePeriod)
+            {
+                WriteMessage();
+                previousScanTime = UnityEngine.Time.time;
+            }
+        }
+
+        private void WriteMessage() {
+            message.latitude = Mathf.Round((c.linear_vel.x + SimUtils.getRandNormal(0, velocityNoiseStdDev) * c.Power / 4.0f) * 1000f) / 1000f;
+            message.longitude = Mathf.Round((c.linear_vel.z + SimUtils.getRandNormal(0, velocityNoiseStdDev) * c.Power / 4.0f) * 1000f) / 1000f;
+            Publish(message);
+        }
+    }
+}

Unity/Assets/RosSharpModules/VelocityPublisher.cs

@@ -37,10 +37,10 @@ protected override void Start()
 
         private void FixedUpdate()
         {
-            if (UnityEngine.Time.realtimeSinceStartup >= previousScanTime + updatePeriod)
+            if (UnityEngine.Time.time >= previousScanTime + updatePeriod)
             {
                 WriteMessage();
-                previousScanTime = UnityEngine.Time.realtimeSinceStartup;
+                previousScanTime = UnityEngine.Time.time;
             }
         }
 

Unity/Assets/Scenes/MainScene.unity

@@ -675,6 +675,8 @@ GameObject:
   - component: {fileID: 829615099}
   - component: {fileID: 829615096}
   - component: {fileID: 829615097}
+  - component: {fileID: 829615101}
+  - component: {fileID: 829615100}
   - component: {fileID: 829615095}
   - component: {fileID: 829615092}
   - component: {fileID: 829615091}
@@ -995,6 +997,34 @@ MonoBehaviour:
   m_EditorClassIdentifier: 
   Topic: /truth/gps
   noNoiseOverride: 1
+--- !u!114 &829615100
+MonoBehaviour:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 829615084}
+  m_Enabled: 1
+  m_EditorHideFlags: 0
+  m_Script: {fileID: 11500000, guid: 6d053faad01c76040ae69950cff1fa28, type: 3}
+  m_Name: 
+  m_EditorClassIdentifier: 
+  Topic: /truth/linear_velocity
+  noNoiseOverride: 1
+--- !u!114 &829615101
+MonoBehaviour:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 829615084}
+  m_Enabled: 1
+  m_EditorHideFlags: 0
+  m_Script: {fileID: 11500000, guid: 6d053faad01c76040ae69950cff1fa28, type: 3}
+  m_Name: 
+  m_EditorClassIdentifier: 
+  Topic: /sim/linear_velocity
+  noNoiseOverride: 0
 --- !u!1 &874833490
 GameObject:
   m_ObjectHideFlags: 0

Unity/Assets/Scripts/ConfigLoader.cs

@@ -35,6 +35,7 @@ public static void LoadConfig()
             simulator.ManualTopSpeed = cfg["Simulator"]["ManualTopSpeed"].FloatValue;
             simulator.RosBridgeServerUrl = cfg["Simulator"]["RosBridgeUrl"].StringValue;
             simulator.Seed = cfg["Simulator"]["Seed"].IntValue;
+            simulator.TimeScale = cfg["Simulator"]["TimeScale"].FloatValue;
 
             simulator.CompetitionMode = cfg["Competition"]["CompetitionMode"].BoolValue;
             simulator.MaxTime = cfg["Competition"]["MaxTime"].FloatValue;
@@ -102,6 +103,7 @@ public class SimulatorConfig
         public int Seed = -1;
 
         public float MaxTime = 300f;
+        public float TimeScale = 1.0f;
     }
 
     public class CompetitionConfig

Unity/Assets/Scripts/GameManager.cs

@@ -81,6 +81,8 @@ void Initalize()
 
         maxTime = ConfigLoader.simulator.MaxTime;
 
+        UnityEngine.Time.timeScale = ConfigLoader.simulator.TimeScale;
+
         waypoints.Add(new Vector2(-37, 0)); // Start Pos
 
         UnityEngine.Random.InitState(ConfigLoader.simulator.Seed);

Unity/ProjectSettings/ProjectSettings.asset

@@ -124,7 +124,7 @@ PlayerSettings:
     16:10: 1
     16:9: 1
     Others: 1
-  bundleVersion: 5.1
+  bundleVersion: 6.0
   preloadedAssets: []
   metroInputSource: 0
   wsaTransparentSwapchain: 0

simulator.cfg

@@ -19,6 +19,9 @@ ManualTopSpeed=2
 # Disabling can significantly improve simulator performance
 EnableCamera=false
 
+# Time Scale (1.0 = normal time, 2.0 = double speed, etc.)
+TimeScale=1.0
+
 [Competition]
 # Competition mode is true when running the competition. It does the following things:
 # - Manual mode is forced off