diff --git a/Ros2Conversions.cs b/Ros2Conversions.cs index cb0bd60..f9b943b 100644 --- a/Ros2Conversions.cs +++ b/Ros2Conversions.cs @@ -523,34 +523,12 @@ public static Detected3DObjectArray ConvertTo(Detection3DArray data) public static Data.VehicleControlData ConvertTo(Data.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 Data.VehicleControlData() { 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, SteerInput = data.target_wheel_angular_rate, TargetGear = (GearPosition)(int)data.target_gear, };