Added bumper & velocity publisher, control noise. Improved results ou…

…tput. Changed score balance.

- Added bumper publisher and velocity publisher. Velocity publisher has noise.
- Added noise to control. No more perfect localization.
- Improved results.txt output including info for seed, time, and score.
- Obstacles: none from 8x to 15x, hard from 0.4x to 0.1x.
- Noise: none from 8x to 10x.

Unity/Assets/RosSharpModules/BumperPublisher.cs

@@ -0,0 +1,24 @@
+using UnityEngine;
+
+namespace RosSharp.RosBridgeClient.MessageTypes.Std
+{
+    public class BumperPublisher : UnityPublisher<Bool>
+    {
+        private Bool message;
+        protected override void Start()
+        {
+            base.Start();
+            message = new Bool();
+        }
+
+        private void OnTriggerEnter(Collider other) {
+            message.data = true;
+            Publish(message);
+        }
+
+        private void OnTriggerExit(Collider other) {
+            message.data = false;
+            Publish(message);
+        }
+    }
+}

Unity/Assets/RosSharpModules/ControlSubscriber.cs

@@ -7,6 +7,9 @@ public class ControlSubscriber : UnitySubscriber<Control>
     {
         private AckermannController car;
 
+        private float angleNoiseStdDev = 0.4f;
+        private float powerNoiseStdDev = 0.1f;
+
         private bool firstMessage = true;
         private bool begingame = false;
 
@@ -15,6 +18,17 @@ protected override void Start()
             if (ConfigLoader.simulator.ManualControl)
                 return;
 
+            switch (ConfigLoader.competition.NoiseLevel) {
+                case ConfigLoader.CompetitionConfig.NoiseLevels.none:
+                    angleNoiseStdDev *= 0;
+                    powerNoiseStdDev *= 0;
+                    break;
+                case ConfigLoader.CompetitionConfig.NoiseLevels.reduced:
+                    angleNoiseStdDev *= 0.5f;
+                    powerNoiseStdDev *= 0.5f;
+                    break;
+            }
+
             base.Start();
             car = GetComponent<AckermannController>();
         }
@@ -30,8 +44,8 @@ private void Update()
 
         protected override void ReceiveMessage(Control control)
         {
-            car.CntrlAngle = control.turn_angle;
-            car.CntrlPower = control.speed;
+            car.CntrlAngle = control.turn_angle + SimUtils.getRandNormal(0, angleNoiseStdDev);
+            car.CntrlPower = control.speed + SimUtils.getRandNormal(0, powerNoiseStdDev);
 
             if (firstMessage)
             {

Unity/Assets/RosSharpModules/GPSPublisher.cs

@@ -6,14 +6,14 @@ public class GPSPublisher : UnityPublisher<Gps>
     {
         private Gps message;
 
-        public float latNoiseStdDev = 1.843f;
-        public float lonNoiseStdDev = 2.138f;
+        private float latNoiseStdDev = 1.843f;
+        private float lonNoiseStdDev = 2.138f;
 
-        public float lat0Pos = 35.205853f;
-        public float lon0Pos = -97.442325f;
+        private float lat0Pos = 35.205853f;
+        private float lon0Pos = -97.442325f;
 
         private float previousScanTime = 5;
-        public float updatePeriod = 0.1f;
+        private float updatePeriod = 0.1f;
 
         protected override void Start()
         {

Unity/Assets/RosSharpModules/IMUPublisher.cs

@@ -7,14 +7,14 @@ public class IMUPublisher : UnityPublisher<Imu>
     {
         private Imu message;
 
-        public float accelNoiseStdDev = 0.15f;
-        public float orientationNoiseStdDev = 0.017f;
-        public float angularVelocityNoiseStdDev = 0.017f;
+        private float accelNoiseStdDev = 0.15f;
+        private float orientationNoiseStdDev = 0.017f;
+        private float angularVelocityNoiseStdDev = 0.017f;
 
         private AckermannController c;
 
         private float previousScanTime = 0;
-        public float updatePeriod = 0.1f;
+        private float updatePeriod = 0.1f;
 
         protected override void Start()
         {

Unity/Assets/RosSharpModules/VelocityPublisher.cs

@@ -0,0 +1,46 @@
+using UnityEngine;
+
+namespace RosSharp.RosBridgeClient.MessageTypes.Std
+{
+    [RequireComponent(typeof(AckermannController))]
+    public class VelocityPublisher : UnityPublisher<Float32>
+    {
+        private Float32 message;
+
+        private float velocityNoiseStdDev = 0.1f;
+
+        private float previousScanTime = 5;
+        private float updatePeriod = 0.05f;
+        private AckermannController c;
+
+        protected override void Start()
+        {
+            base.Start();
+            c = GetComponent<AckermannController>();
+            message = new Float32();
+
+            switch (ConfigLoader.competition.NoiseLevel) {
+                case ConfigLoader.CompetitionConfig.NoiseLevels.none:
+                    velocityNoiseStdDev *= 0;
+                    break;
+                case ConfigLoader.CompetitionConfig.NoiseLevels.reduced:
+                    velocityNoiseStdDev *= 0.5f;
+                    break;
+            }
+        }
+
+        private void FixedUpdate()
+        {
+            if (UnityEngine.Time.realtimeSinceStartup >= previousScanTime + updatePeriod)
+            {
+                WriteMessage();
+                previousScanTime = UnityEngine.Time.realtimeSinceStartup;
+            }
+        }
+
+        private void WriteMessage() {
+            message.data = Mathf.Round((c.Power + SimUtils.getRandNormal(0, velocityNoiseStdDev) * c.Power / 4.0f) * 1000f) / 1000f;
+            Publish(message);
+        }
+    }
+}

Unity/Assets/Scenes/MainScene.unity

@@ -178,6 +178,50 @@ MonoBehaviour:
   density: 0.35
   seed: 0
   freeArea: 1
+--- !u!1 &215013118
+GameObject:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  serializedVersion: 6
+  m_Component:
+  - component: {fileID: 215013119}
+  - component: {fileID: 215013120}
+  m_Layer: 0
+  m_Name: FrontBumper
+  m_TagString: Untagged
+  m_Icon: {fileID: 0}
+  m_NavMeshLayer: 0
+  m_StaticEditorFlags: 0
+  m_IsActive: 1
+--- !u!4 &215013119
+Transform:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 215013118}
+  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: 829615085}
+  m_RootOrder: 9
+  m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
+--- !u!65 &215013120
+BoxCollider:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 215013118}
+  m_Material: {fileID: 0}
+  m_IsTrigger: 1
+  m_Enabled: 1
+  serializedVersion: 2
+  m_Size: {x: 0.18, y: 0.1, z: 0.3}
+  m_Center: {x: 0.38, y: 0.15, z: 0}
 --- !u!1 &360597449
 GameObject:
   m_ObjectHideFlags: 0
@@ -627,6 +671,8 @@ GameObject:
   - component: {fileID: 829615087}
   - component: {fileID: 829615089}
   - component: {fileID: 829615090}
+  - component: {fileID: 829615096}
+  - component: {fileID: 829615095}
   - component: {fileID: 829615092}
   - component: {fileID: 829615091}
   - component: {fileID: 829615093}
@@ -658,6 +704,7 @@ Transform:
   - {fileID: 688134255}
   - {fileID: 1065359072}
   - {fileID: 952696888}
+  - {fileID: 215013119}
   m_Father: {fileID: 0}
   m_RootOrder: 2
   m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0}
@@ -725,10 +772,6 @@ MonoBehaviour:
   m_Name: 
   m_EditorClassIdentifier: 
   Topic: /sim/imu
-  accelNoiseStdDev: 0.15
-  orientationNoiseStdDev: 0.017
-  angularVelocityNoiseStdDev: 0.017
-  updatePeriod: 0.04
 --- !u!114 &829615090
 MonoBehaviour:
   m_ObjectHideFlags: 0
@@ -742,11 +785,6 @@ MonoBehaviour:
   m_Name: 
   m_EditorClassIdentifier: 
   Topic: /sim/gps
-  latNoiseStdDev: 1.843
-  lonNoiseStdDev: 2.138
-  lat0Pos: 35.205853
-  lon0Pos: -97.44232
-  updatePeriod: 0.1
 --- !u!114 &829615091
 MonoBehaviour:
   m_ObjectHideFlags: 0
@@ -889,6 +927,32 @@ ConfigurableJoint:
   m_EnablePreprocessing: 1
   m_MassScale: 1
   m_ConnectedMassScale: 1
+--- !u!114 &829615095
+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: 6754bb5aaed72614b8f93c1d3ff3a992, type: 3}
+  m_Name: 
+  m_EditorClassIdentifier: 
+  Topic: /sim/bumper
+--- !u!114 &829615096
+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: c5970dc0eb3926240b605e471f7eeced, type: 3}
+  m_Name: 
+  m_EditorClassIdentifier: 
+  Topic: /sim/velocity
 --- !u!1 &874833490
 GameObject:
   m_ObjectHideFlags: 0
@@ -1078,7 +1142,7 @@ MonoBehaviour:
   m_Name: 
   m_EditorClassIdentifier: 
   samples: 360
-  update_rate: 3600
+  update_rate: 7200
   angle_min: 0
   angle_max: 6.28
   angle_increment: 0.0174533

Unity/Assets/Scripts/ConfigLoader.cs

@@ -113,27 +113,27 @@ public enum NoiseLevels { none, reduced, realistic };
         public float getObstacleScore() {
             switch (Obstacles) {
                 case ObstacleTypes.none:
-                    return 8.0f;
+                    return 15.0f;
                 case ObstacleTypes.normal:
                     return 1.0f;
                 case ObstacleTypes.hard:
-                    return 0.4f;
+                    return 0.1f;
             }
 
-            return 10f; // if error, assume they're cheating
+            return 100f; // if error, assume they're cheating
         }
 
         public float getNoiseScore() {
             switch (NoiseLevel) {
                 case NoiseLevels.none:
-                    return 8.0f;
+                    return 10.0f;
                 case NoiseLevels.reduced:
                     return 1.0f;
                 case NoiseLevels.realistic:
                     return 0.6f;
             }
 
-            return 10f; // if error, assume they're cheating
+            return 100f; // if error, assume they're cheating
         }
     }
 

Unity/Assets/Scripts/FieldBuilder.cs

@@ -100,11 +100,11 @@ void Start()
             frontier.Remove(cell);
         }
 
-        Debug.Log("width: " + width + ", height: " + height);
+        //Debug.Log("width: " + width + ", height: " + height);
 
         foreach (Vector2 waypoint in waypoints) {
             (int, int) wpt = waypointToTuple(waypoint);
-            Debug.Log(wpt);
+            //Debug.Log(wpt);
             for (int i=-freeArea; i<=freeArea; i++) {
                 for (int j=-freeArea; j<=freeArea; j++) {
                     if (wpt.Item1 + i >= 0 && wpt.Item1 + i < width && wpt.Item2 + j >= 0 && wpt.Item2 + j <= height)

Unity/Assets/Scripts/GameManager.cs

@@ -193,11 +193,25 @@ public void StopSim(string error)
             Application.Quit();
         }
 
-        writer.WriteLine(finalTime);
+        float scoreMultiplier = 1f;
 
         foreach (ScoreAdjust score in adjustments)
         {
-            writer.WriteLine(score.adjustment + ": " + score.reason);
+            scoreMultiplier *= score.adjustment;
+        }
+
+        writer.WriteLine($"Team {ConfigLoader.competition.Name}");
+        writer.WriteLine($"Date: {System.DateTime.Now.ToString("yyyy-MM-dd HH:mm:ss zz")}");
+        writer.WriteLine($"Seed: {ConfigLoader.simulator.Seed}");
+        writer.WriteLine();
+        writer.WriteLine($"Time: {finalTime:0.000}");
+        writer.WriteLine($"Multiplier: x{scoreMultiplier:0.000}");
+        writer.WriteLine($"Score: {(finalTime * scoreMultiplier):0.000}");
+        writer.WriteLine();
+        writer.WriteLine("-- Multipliers --");
+        foreach (ScoreAdjust score in adjustments)
+        {
+            writer.WriteLine($"{score.reason}: x{score.adjustment:0.00}");
         }
 
         writer.Close();

Unity/ProjectSettings/ProjectSettings.asset

@@ -119,7 +119,7 @@ PlayerSettings:
     16:10: 1
     16:9: 1
     Others: 1
-  bundleVersion: 1.0
+  bundleVersion: 2.0
   preloadedAssets: []
   metroInputSource: 0
   wsaTransparentSwapchain: 0