diff --git a/Assets/Scripts/Bridge/Ros2/Ros2Conversions.cs b/Assets/Scripts/Bridge/Ros2/Ros2Conversions.cs index 7721b044c..237f78c39 100644 --- a/Assets/Scripts/Bridge/Ros2/Ros2Conversions.cs +++ b/Assets/Scripts/Bridge/Ros2/Ros2Conversions.cs @@ -368,26 +368,6 @@ public static Detected3DObjectArray ConvertTo(Lgsvl.Detection3DArray data) public static VehicleControlData ConvertTo(Lgsvl.VehicleControlData data) { - float Deg2Rad = UnityEngine.Mathf.Deg2Rad; - float MaxSteeringAngle = 39.4f * Deg2Rad; - float wheelAngle = 0f; - - if (data.target_wheel_angle > MaxSteeringAngle) - { - wheelAngle = MaxSteeringAngle; - } - else if (data.target_wheel_angle < -MaxSteeringAngle) - { - wheelAngle = -MaxSteeringAngle; - } - else - { - wheelAngle = data.target_wheel_angle; - } - - // ratio between -MaxSteeringAngle and MaxSteeringAngle - var k = (float)(wheelAngle + MaxSteeringAngle) / (MaxSteeringAngle*2); - // target_gear are not supported on simulator side return new VehicleControlData() @@ -395,7 +375,7 @@ public static VehicleControlData ConvertTo(Lgsvl.VehicleControlData data) TimeStampSec = Convert(data.header.stamp), Acceleration = data.acceleration_pct, Braking = data.braking_pct, - SteerAngle = UnityEngine.Mathf.Lerp(-1f, 1f, k), + SteerAngle = data.target_wheel_angle * UnityEngine.Mathf.Rad2Deg, SteerInput = data.target_wheel_angular_rate, }; }