From 04320aff7f849d5c65fe82ded1299dc4aacd1a5c Mon Sep 17 00:00:00 2001 From: Noah Zemlin Date: Tue, 17 Nov 2020 18:35:47 -0600 Subject: [PATCH] Added Linear Velocity publishing. Added TimeScale configuration. --- Unity/Assets/RosSharpModules/GPSPublisher.cs | 4 +- Unity/Assets/RosSharpModules/IMUPublisher.cs | 4 +- .../LinearVelocityPublisher.cs | 54 +++++++++++++++++++ .../LinearVelocityPublisher.cs.meta | 11 ++++ .../RosSharpModules/VelocityPublisher.cs | 4 +- Unity/Assets/Scenes/MainScene.unity | 30 +++++++++++ Unity/Assets/Scripts/ConfigLoader.cs | 2 + Unity/Assets/Scripts/GameManager.cs | 2 + Unity/ProjectSettings/ProjectSettings.asset | 2 +- simulator.cfg | 3 ++ 10 files changed, 109 insertions(+), 7 deletions(-) create mode 100644 Unity/Assets/RosSharpModules/LinearVelocityPublisher.cs create mode 100644 Unity/Assets/RosSharpModules/LinearVelocityPublisher.cs.meta diff --git a/Unity/Assets/RosSharpModules/GPSPublisher.cs b/Unity/Assets/RosSharpModules/GPSPublisher.cs index 83c3a41..a759848 100644 --- a/Unity/Assets/RosSharpModules/GPSPublisher.cs +++ b/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; } } diff --git a/Unity/Assets/RosSharpModules/IMUPublisher.cs b/Unity/Assets/RosSharpModules/IMUPublisher.cs index 8cad43f..5d8add3 100644 --- a/Unity/Assets/RosSharpModules/IMUPublisher.cs +++ b/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; } } diff --git a/Unity/Assets/RosSharpModules/LinearVelocityPublisher.cs b/Unity/Assets/RosSharpModules/LinearVelocityPublisher.cs new file mode 100644 index 0000000..f0f2b82 --- /dev/null +++ b/Unity/Assets/RosSharpModules/LinearVelocityPublisher.cs @@ -0,0 +1,54 @@ +using UnityEngine; + +namespace RosSharp.RosBridgeClient.MessageTypes.swc_msgs +{ + [RequireComponent(typeof(AckermannController))] + public class LinearVelocityPublisher : UnityPublisher + { + 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(); + 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); + } + } +} \ No newline at end of file diff --git a/Unity/Assets/RosSharpModules/LinearVelocityPublisher.cs.meta b/Unity/Assets/RosSharpModules/LinearVelocityPublisher.cs.meta new file mode 100644 index 0000000..34229da --- /dev/null +++ b/Unity/Assets/RosSharpModules/LinearVelocityPublisher.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6d053faad01c76040ae69950cff1fa28 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Unity/Assets/RosSharpModules/VelocityPublisher.cs b/Unity/Assets/RosSharpModules/VelocityPublisher.cs index 1dda0ae..4f70b04 100644 --- a/Unity/Assets/RosSharpModules/VelocityPublisher.cs +++ b/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; } } diff --git a/Unity/Assets/Scenes/MainScene.unity b/Unity/Assets/Scenes/MainScene.unity index 58316bf..aea5807 100644 --- a/Unity/Assets/Scenes/MainScene.unity +++ b/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 diff --git a/Unity/Assets/Scripts/ConfigLoader.cs b/Unity/Assets/Scripts/ConfigLoader.cs index c0a4341..ea80906 100644 --- a/Unity/Assets/Scripts/ConfigLoader.cs +++ b/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 diff --git a/Unity/Assets/Scripts/GameManager.cs b/Unity/Assets/Scripts/GameManager.cs index 01b216e..91ebad9 100644 --- a/Unity/Assets/Scripts/GameManager.cs +++ b/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); diff --git a/Unity/ProjectSettings/ProjectSettings.asset b/Unity/ProjectSettings/ProjectSettings.asset index 143465d..539e689 100644 --- a/Unity/ProjectSettings/ProjectSettings.asset +++ b/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 diff --git a/simulator.cfg b/simulator.cfg index bd52cf0..830f407 100644 --- a/simulator.cfg +++ b/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