fix: camera position on reset (#171)

Signed-off-by: Alptuğ Cırıt <[email protected]>
autowarefoundation · Oct 2, 2024 · 4f9bbbb · 4f9bbbb
1 parent b7bf152
commit 4f9bbbb
Show file tree

Hide file tree

Showing 6 changed files with 68 additions and 43 deletions.
diff --git a/Assets/AWSIM/Prefabs/Vehicles/Lexus RX450h 2015 Sample Sensor.prefab b/Assets/AWSIM/Prefabs/Vehicles/Lexus RX450h 2015 Sample Sensor.prefab
@@ -1220,21 +1220,16 @@ MonoBehaviour:
   m_Script: {fileID: -1898320663, guid: b8873f7cd5fec6f49a4b37228b9b23ac, type: 3}
   m_Name: 
   m_EditorClassIdentifier: 
-  integrationSteps: 8
-  integrationUseRK4: 0
   centerOfMass: {fileID: 2963107001226707149}
-  tireSideDeflection: 0
-  tireSideDeflectionRate: 10
-  tireAdherentImpulseRatio: 0.5
-  wheelSleepVelocity: 0.2
-  advancedSuspensionDamper: 1
-  suspensionDamperLimitFactor: 2
+  solverSubsteps: 8
+  wheelMomentumFactor: 0.25
+  wheelMomentumModel: 0
   contactAngleAffectsTireForce: 1
   dontAdjustSuspensionForcePoint: 0
   disableSteerAngleFix: 0
   disableWheelReferenceFrameFix: 0
   scaleFactor: 1
-  vehicleSleepCriteria: 0
+  suspensionDamperLimitFactor: 2
   showContactGizmos: 1
   disableContactProcessing: 0
   inertia:
@@ -1333,6 +1328,7 @@ MonoBehaviour:
     ackerman: 1
     ackermanReference: {fileID: 5055317025945945549}
     ratioReference: {fileID: 0}
+    steeringWheelRange: 900
   brakes:
     maxBrakeTorque: 2800
     brakeBias: 0.68
@@ -1357,7 +1353,6 @@ MonoBehaviour:
       E: -2
       adherent: {x: 0.5, y: -1}
     frictionMultiplier: 1
-    maxAdherentSpeed: 6.388889
   engine:
     idleRpm: 1000
     peakRpm: 7700
@@ -1423,12 +1418,14 @@ MonoBehaviour:
     automaticShiftInterval: 1
     automaticGearDownRevs: 2000
     automaticGearUpRevs: 5000
+    automaticMaxRpm: 5500
     automaticThrottleSens: 0
     automaticThrottleSensMin: 0.6
     automaticFullThrottleGearDownRevs: 3000
     automaticFullThrottleGearUpRevs: 4500
-    automaticGearUpRequiresThrottle: 1
     automaticShiftReverseGears: 0
+    automaticGearUpRequiresThrottle: 1
+    automaticGearUpAboveMaxRpm: 0
     automaticInitialGearForward: 1
     automaticInitialGearReverse: -1
   antiLock:
@@ -1460,11 +1457,11 @@ MonoBehaviour:
     maxSpeed: 5.555556
     minRotationDiffRpm: 100
     maxRotationDiffRpm: 400
-    maxBrakeTorque: 1000
+    maxBrakeRatio: 1
   engineReactionFactor: 1
   parkModeReactionFactor: 0.95
   maxSubsystemsEnergy: 100000
-  "\u202A\u206C\u202D\u206D\u200B\u206E\u202B\u202B\u202D\u206D\u206B\u200E\u206B\u202C\u206B\u202C\u202B\u206F\u202C\u200F\u202B\u202C\u202C\u200B\u206C\u206E\u202A\u202B\u200B\u200B\u200F\u200D\u206F\u202C\u200C\u206F\u202D\u206E\u206D\u206A\u202E": 1
+  "\u200B\u202A\u202B\u200F\u200B\u206F\u206D\u202E\u206D\u202D\u206E\u206E\u206C\u206F\u202B\u200E\u202D\u206E\u206C\u202B\u202D\u200F\u202D\u202D\u200D\u200B\u202D\u206A\u202D\u202B\u200D\u202E\u202E\u202A\u202E\u202A\u206A\u206F\u206A\u206E\u202E": 1
 --- !u!114 &3608516095911096369
 MonoBehaviour:
   m_ObjectHideFlags: 0
@@ -1497,7 +1494,9 @@ MonoBehaviour:
   disableSteerInput: 0
   disableThrottleInput: 0
   disableBrakeInput: 0
+  disableHandbrakeInput: 0
   disableClutchInput: 0
+  disableGearShiftInputs: 0
   externalThrottle: 0
   reverse: 0
   externalBrake: 0
@@ -1557,7 +1556,6 @@ MonoBehaviour:
   showContactPoints: 1
   showTireSlip: 1
   showTireForces: 1
-  showSurfaceForces: 0
   useLogScale: 1
 --- !u!114 &8627087200372202738
 MonoBehaviour:
@@ -1572,7 +1570,6 @@ MonoBehaviour:
   m_Name: 
   m_EditorClassIdentifier: 
   steeringWheel: {fileID: 0}
-  degreesOfRotation: 600
   localRotationAxis: 2
   brakeLightsOn:
   - {fileID: 0}
@@ -1619,7 +1616,7 @@ MonoBehaviour:
   _simulateSteering: 0
   _steerWheelInput: 45
   _emergencyBrakePercent: 100
-  _updatePositionOffsetY: 1.33
+  _updatePositionOffsetY: 0.5
   _updatePositionRayOriginY: 1000
   _doPedalCalibration: 0
 --- !u!114 &8380214914162268112
@@ -3412,13 +3409,15 @@ MonoBehaviour:
   m_Name: 
   m_EditorClassIdentifier: 
   dynamicModel: 0
-  dynamicCoefficient: 0.04
+  constantCoefficient: 0.04
+  linearCoefficient: 0.0003
+  tirePressure: 2
   staticCoefficient: 0.004
   staticSpeedThreshold: 2
   axleFrictionFactors: []
   showDebugLabel: 0
   coefficient: -1
-  version: 1
+  version: 2
 --- !u!114 &7226978134186799435
 MonoBehaviour:
   m_ObjectHideFlags: 0

diff --git a/Assets/AWSIM/Scripts/UI/EgoVehiclePositionManager.cs b/Assets/AWSIM/Scripts/UI/EgoVehiclePositionManager.cs
@@ -1,13 +1,16 @@
+using System;
 using UnityEngine;
-using VehiclePhysics;
 
 namespace AWSIM.Scripts.UI
 {
     public class EgoVehiclePositionManager : MonoBehaviour
     {
         public Transform EgoTransform { private get; set; }
-        private Vector3 initialEgoPosition;
-        private Quaternion initialEgoRotation;
+        private Vector3 _initialEgoPosition;
+        private Quaternion _initialEgoRotation;
+
+        // Event to notify subscribers that the ego vehicle has been reset to the spawn point.
+        public event Action<Vector3, Quaternion> OnEgoReset;
 
         private void Start()
         {
@@ -23,8 +26,8 @@ public void Activate()
         private void InitializeEgoTransform(Transform egoTransform)
         {
             EgoTransform = egoTransform;
-            initialEgoPosition = EgoTransform.position;
-            initialEgoRotation = EgoTransform.rotation;
+            _initialEgoPosition = EgoTransform.position;
+            _initialEgoRotation = EgoTransform.rotation;
         }
 
         // If the ego transform reference is present, reset the ego to the initial position and rotation.
@@ -36,8 +39,8 @@ public void ResetEgoToSpawnPoint()
                 return;
             }
 
-            var vpController = EgoTransform.GetComponent<VPVehicleController>();
-            vpController.HardReposition(initialEgoPosition, initialEgoRotation);
+            // Reset the ego vehicle to the initial position and rotation.
+            OnEgoReset?.Invoke(_initialEgoPosition, _initialEgoRotation);
         }
     }
 }
diff --git a/Assets/AWSIM/Scripts/UI/VehicleDashboard.cs b/Assets/AWSIM/Scripts/UI/VehicleDashboard.cs
@@ -15,7 +15,6 @@ namespace AWSIM.Scripts.UI
     public class VehicleDashboard : MonoBehaviour
     {
         private VPVehicleController _vehicleController;
-        private int[] _vehicleDataBus;
         private AutowareVPPAdapter _adapter;
         [Header("Speed")]
         [SerializeField] private Text _speedText;
@@ -39,26 +38,21 @@ public class VehicleDashboard : MonoBehaviour
 
         private const float MsToKmH = 3.6f;
 
-        private void Start()
+        private void GetVariables()
         {
             _vehicleController = FindObjectOfType<VPVehicleController>();
-            _vehicleDataBus = _vehicleController.data.bus[Channel.Vehicle];
             _adapter = FindObjectOfType<AutowareVPPAdapter>();
-
-            if (_vehicleController == null || _adapter == null)
-            {
-                Debug.LogError("VehicleController or AutowareVPPAdapter component not found!");
-            }
         }
 
         public void Activate()
         {
-            Start();
+            GetVariables();
         }
 
         private void FixedUpdate()
         {
             if (_vehicleController == null) return;
+            if (_adapter == null) return;
 
             UpdateDashboard();
         }
@@ -67,12 +61,12 @@ private void UpdateDashboard()
         {
             _speedText.text = UpdateSpeed(_vehicleController);
             // _transmissionModeText.text = UpdateTransmissionMode(_vehicleDataBus[VehicleData.GearboxMode]);
-            _gearText.text = UpdateGear(_vehicleDataBus[VehicleData.GearboxGear]);
+            _gearText.text = UpdateGear(_vehicleController.data.bus[Channel.Vehicle][VehicleData.GearboxGear]);
 
-            UpdateSystemState(_vehicleDataBus[VehicleData.AbsEngaged], _absText);
-            UpdateSystemState(_vehicleDataBus[VehicleData.AsrEngaged], _asrText);
-            UpdateSystemState(_vehicleDataBus[VehicleData.EscEngaged], _escText);
-            UpdateSystemState(_vehicleDataBus[VehicleData.TcsEngaged], _tcsText);
+            UpdateSystemState(_vehicleController.data.bus[Channel.Vehicle][VehicleData.AbsEngaged], _absText);
+            UpdateSystemState(_vehicleController.data.bus[Channel.Vehicle][VehicleData.AsrEngaged], _asrText);
+            UpdateSystemState(_vehicleController.data.bus[Channel.Vehicle][VehicleData.EscEngaged], _escText);
+            UpdateSystemState(_vehicleController.data.bus[Channel.Vehicle][VehicleData.TcsEngaged], _tcsText);
 
             // UpdateControlMode();
             UpdateControlModeImage();
@@ -166,7 +160,7 @@ private string UpdateGear(int gearVal)
             return gearVal switch
             {
                 < 0 => "R",
-                0 => _vehicleDataBus[VehicleData.GearboxMode] switch
+                0 => _vehicleController.data.bus[Channel.Vehicle][VehicleData.GearboxMode] switch
                 {
                     (int)Gearbox.AutomaticGear.P => "P",
                     (int)Gearbox.AutomaticGear.N => "N",

diff --git a/Assets/AWSIM/Scripts/UI/VehicleDashboard.cs.meta b/Assets/AWSIM/Scripts/UI/VehicleDashboard.cs.meta
diff --git a/Assets/AWSIM/Scripts/Vehicles/VPP Integration/AutowareVPPAdapter.cs b/Assets/AWSIM/Scripts/Vehicles/VPP Integration/AutowareVPPAdapter.cs
@@ -1,5 +1,6 @@
 using System;
 using System.Collections.Generic;
+using AWSIM.Scripts.UI;
 using AWSIM.Scripts.Vehicles.VPP_Integration.Enums;
 using AWSIM.Scripts.Vehicles.VPP_Integration.IVehicleControlModes;
 using UnityEngine;
@@ -130,7 +131,7 @@ public class AutowareVPPAdapter : MonoBehaviour
         // RViz2 Update position variables
         [Header("RViz2 Update Position")]
         [SerializeField]
-        private float _updatePositionOffsetY = 1.33f;
+        private float _updatePositionOffsetY = 0.5f;
 
         [SerializeField] private float _updatePositionRayOriginY = 1000f;
 
@@ -147,6 +148,9 @@ public class AutowareVPPAdapter : MonoBehaviour
         private const float VppToAutowareMultiplier = 0.0001f;
         private const int AutowareToVppMultiplier = 10000;
 
+        // Event holders to subscribe
+        private EgoVehiclePositionManager _egoVehiclePositionManager;
+
         private void Awake()
         {
             //TODO: Implement the control mode switch from simulator (mozzz)
@@ -172,6 +176,13 @@ private void Start()
             // reset vpp stuff for multi-scene loading (no idea why, but it works)
             _vehicleController.enabled = false;
             _vehicleController.enabled = true;
+
+            // Subscribe to events
+            _egoVehiclePositionManager = FindObjectOfType<EgoVehiclePositionManager>();
+            if (_egoVehiclePositionManager != null)
+            {
+                _egoVehiclePositionManager.OnEgoReset += ResetEgoPosition;
+            }
         }
 
         // private void Update()
@@ -341,6 +352,8 @@ private void UpdateEgoPosition()
             {
                 PositionInput = new Vector3(PositionInput.x, hit.point.y + _updatePositionOffsetY, PositionInput.z);
                 _vehicleController.HardReposition(PositionInput, RotationInput);
+                _vehicleController.data.bus[Channel.Input][InputData.AutomaticGear] = (int)Gearbox.AutomaticGear.P;
+                _cameraController.ResetCamera();
             }
             else
             {
@@ -349,6 +362,16 @@ private void UpdateEgoPosition()
             }
         }
 
+        /// <summary>
+        /// Reset the ego vehicle to the spawn point.
+        /// </summary>
+        private void ResetEgoPosition(Vector3 position, Quaternion rotation)
+        {
+            _vehicleController.HardReposition(position, rotation, true);
+            _vehicleController.data.bus[Channel.Input][InputData.AutomaticGear] = (int)Gearbox.AutomaticGear.P;
+            _cameraController.ResetCamera();
+        }
+
         // TODO: Method to switch control mode based on user input (mozzz)
         // Currently it is updated from UI with keyboard toggle.
         private void UserSwitchControlMode()
@@ -380,5 +403,11 @@ private void PedalCalibrationMode()
                 _brakeAmount = 0;
             }
         }
+
+        private void OnDestroy()
+        {
+            // Unsubscribe from events
+            _egoVehiclePositionManager.OnEgoReset -= ResetEgoPosition;
+        }
     }
 }
diff --git a/Assets/AWSIM/Scripts/Vehicles/VPP Integration/AutowareVPPAdapter.cs.meta b/Assets/AWSIM/Scripts/Vehicles/VPP Integration/AutowareVPPAdapter.cs.meta