Merge pull request #369 from tier4/feat/lag_for_input

feat: first order lag for vehicle control

Assets/AWSIM/Scripts/Vehicles/FirstOrderLaggedFloat.cs

@@ -0,0 +1,59 @@
+using System;
+using UnityEngine;
+
+namespace AWSIM
+{
+    public class FirstOrderLaggedFloat
+    {
+        private float timeConstant;
+
+        private float desiredValue;
+        private float currentValue;
+        private float lastTime;
+
+        public float Value
+        {
+            get
+            {
+                Update();
+                return currentValue;
+            }
+        }
+        public float DesiredValue
+        {
+            set
+            {
+                Update();
+                desiredValue = value;
+            }
+            get
+            {
+                return desiredValue;
+            }
+        }
+
+        public FirstOrderLaggedFloat(float timeConstant, float initialValue)
+        {
+            this.timeConstant = timeConstant;
+
+            desiredValue = currentValue = initialValue;
+            lastTime = Time.time;
+        }
+
+        private void Update()
+        {
+            float dt = Time.time - lastTime;
+
+            if (timeConstant == 0.0f)
+            {
+                currentValue = desiredValue;
+            }
+            else
+            {
+                currentValue += (dt / timeConstant) * (desiredValue - currentValue);
+            }
+
+            lastTime = Time.time;
+        }
+    }
+}

Assets/AWSIM/Scripts/Vehicles/Vehicle.cs

@@ -171,11 +171,15 @@ public float SidewaySlipMultipler
         // -MaxSteerAngleInput <= SteerAngleInput <= MaxSteerAngleInput.
         [Range(0.01f, 80)]
         public float MaxSteerAngleInput = 25f;
+        [SerializeField, Min(0.0f), Tooltip("Set 0 to disable the lag")]
+        private float steerAngleTimeConstant = 0.0f;
 
         // Set value to clamp AccelerationInput (m/s^2).
         // -MaxAccelerationInput <= AccelerationInput <= MaxAccelerationInput.
         [Range(0.01f, 50)]
         public float MaxAccelerationInput = 10;
+        [SerializeField, Min(0.0f), Tooltip("Set 0 to disable the lag")]
+        private float accelerationTimeConstant = 0.0f;
 
         [Header("Inputs")]
 
@@ -189,15 +193,23 @@ public float SidewaySlipMultipler
         /// In the plane, output the force that will result in this acceleration.
         /// On a slope, it is affected by the slope resistance, so it does not match the input.
         /// </summary>
-        // TODO: Compute first order lag
-        public float AccelerationInput;
+        public float AccelerationInput
+        {
+            get => acceleration.DesiredValue;
+            set => acceleration.DesiredValue = Mathf.Clamp(value, -MaxAccelerationInput, MaxAccelerationInput);
+        }
+        private FirstOrderLaggedFloat acceleration;
 
         /// <summary>
         /// Vehicle steering input. Tire angle (degree)
         /// Negative is left, positive is right turn tire angle.
         /// </summary>
-        // TODO: Compute first order lag
-        public float SteerAngleInput;
+        public float SteerAngleInput
+        {
+            get => steerAngle.DesiredValue;
+            set => steerAngle.DesiredValue = Mathf.Clamp(value, -MaxSteerAngleInput, MaxSteerAngleInput);
+        }
+        private FirstOrderLaggedFloat steerAngle;
 
         /// <summary>
         /// Vehicle turn signal input. NONE, LEFT, RIGHT, HAZARD.
@@ -209,6 +221,11 @@ public float SidewaySlipMultipler
         /// </summary>
         public Vector3 LocalAcceleration { get; private set; }
 
+        /// <summary>
+        /// Vehicle acceleration (m/s^2)
+        /// </summary>
+        public float Acceleration => acceleration.Value;
+
         /// <summary>
         /// Vehicle speed (m/s)
         /// </summary>
@@ -217,7 +234,7 @@ public float SidewaySlipMultipler
         /// <summary>
         /// Vehicle steering angle (degree)
         /// </summary>
-        public float SteerAngle => SteerAngleInput;
+        public float SteerAngle => steerAngle.Value;
 
         public float SteerAngleNormalized => SteerAngle / MaxSteerAngleInput;
 
@@ -296,6 +313,12 @@ void Awake()
             // Initialize with non-slip value
             ForwardSlipMultipler = 1f;
             SidewaySlipMultipler = 1f;
+
+            // Initialize acceleration
+            acceleration = new FirstOrderLaggedFloat(accelerationTimeConstant, 0.0f);
+
+            // Initialize steer angle
+            steerAngle = new FirstOrderLaggedFloat(steerAngleTimeConstant, 0.0f);
         }
 
         // GroundSlipMultiplier changes the slip rate.
@@ -322,10 +345,6 @@ private void OnTriggerExit(Collider other)
 
         void FixedUpdate()
         {
-            // Clamp input values.
-            AccelerationInput = Mathf.Clamp(AccelerationInput, -MaxAccelerationInput, MaxAccelerationInput);
-            SteerAngleInput = Mathf.Clamp(SteerAngleInput, -MaxSteerAngleInput, MaxSteerAngleInput);
-
             // Compute vehicle infomation.
             ComputeVehicleState();
 
@@ -339,8 +358,7 @@ void FixedUpdate()
             if (sleep == false)
             {
                 // Update wheel force.
-                var acceleration = AccelerationInput;
-                UpdateWheelsForce(acceleration);
+                UpdateWheelsForce(Acceleration);
             }
 
             // cache value for next frame.