From 4f9bbbbd81f1a54bbfd361b346365e24e6c1cd3e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alptu=C4=9F=20C=C4=B1r=C4=B1t?= <33583972+mozhoku@users.noreply.github.com> Date: Wed, 2 Oct 2024 11:15:45 +0300 Subject: [PATCH] fix: camera position on reset (#171) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alptuğ Cırıt --- .../Lexus RX450h 2015 Sample Sensor.prefab | 35 +++++++++---------- .../Scripts/UI/EgoVehiclePositionManager.cs | 17 +++++---- Assets/AWSIM/Scripts/UI/VehicleDashboard.cs | 24 +++++-------- .../AWSIM/Scripts/UI/VehicleDashboard.cs.meta | 2 +- .../VPP Integration/AutowareVPPAdapter.cs | 31 +++++++++++++++- .../AutowareVPPAdapter.cs.meta | 2 +- 6 files changed, 68 insertions(+), 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 index e198bad43..713546985 100644 --- 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 index 4269071f1..1cc93984b 100644 --- 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 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(); - 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 index e029f056a..a57883086 100644 --- 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(); - _vehicleDataBus = _vehicleController.data.bus[Channel.Vehicle]; _adapter = FindObjectOfType(); - - 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 index af478b9b7..d2ec898ef 100644 --- a/Assets/AWSIM/Scripts/UI/VehicleDashboard.cs.meta +++ b/Assets/AWSIM/Scripts/UI/VehicleDashboard.cs.meta @@ -4,7 +4,7 @@ MonoImporter: externalObjects: {} serializedVersion: 2 defaultReferences: [] - executionOrder: 0 + executionOrder: 8 icon: {instanceID: 0} userData: assetBundleName: diff --git a/Assets/AWSIM/Scripts/Vehicles/VPP Integration/AutowareVPPAdapter.cs b/Assets/AWSIM/Scripts/Vehicles/VPP Integration/AutowareVPPAdapter.cs index b9488cfc9..badea5750 100644 --- 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(); + 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() } } + /// + /// Reset the ego vehicle to the spawn point. + /// + 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 index 58b745320..a46d8afa8 100644 --- a/Assets/AWSIM/Scripts/Vehicles/VPP Integration/AutowareVPPAdapter.cs.meta +++ b/Assets/AWSIM/Scripts/Vehicles/VPP Integration/AutowareVPPAdapter.cs.meta @@ -4,7 +4,7 @@ MonoImporter: externalObjects: {} serializedVersion: 2 defaultReferences: [] - executionOrder: 0 + executionOrder: 5 icon: {instanceID: 0} userData: assetBundleName: