public override void OnBridgeSetup(IBridge bridge)
{
bridge.AddReader <SnowballControlData>(Topic, data =>
{
// Debug.Log($"SnowballControlData steer = {data.steering_angle} acc: {data.acceleration} received");
LastControlUpdate = Time.time;
controlData = data;
// Steering (%)
SteerInput = -(float)(data.steering_angle / Dynamics.SteeringRatio) / Dynamics.maxSteeringAngle;
SteerInput = Mathf.Clamp(SteerInput, -1.0f, 1.0f);
// H7 ACC Emulator
// Converts Acceleration Command (m/s^2) to throttle & brake
// Which gets repacked into a single 'AccelInput' that is consumed by Vehicle Dynamics
float inputAccel = (float)data.acceleration;
float currentSpeed = RigidBody.velocity.magnitude;;
var throttle = H2VechicleControl_.ConvertToThrottle(currentSpeed, inputAccel, ImuSensor.currentAccel);
if (inputAccel > 0)
{
AccelInput = throttle;
}
else
{
AccelInput = -H2VechicleControl_.ConvertToBrake(inputAccel);
}
});
}
public override void OnBridgeSetup(IBridge bridge)
{
bridge.AddReader <VehicleControlData>(Topic, data =>
{
controlData = data;
LastControlUpdate = Time.time;
if (data.Velocity.HasValue) // autoware
{
controlType = ControlType.Autoware;
if (data.ShiftGearUp || data.ShiftGearDown)
{
if (data.ShiftGearUp)
{
Dynamics.GearboxShiftUp();
}
if (data.ShiftGearDown)
{
Dynamics.GearboxShiftDown();
}
ADAccelInput = data.Acceleration.GetValueOrDefault() - data.Breaking.GetValueOrDefault(); // converted from lin accel
ADSteerInput = data.SteerAngle.GetValueOrDefault(); // angle should be in degrees
}
else
{
if (Dynamics.Reverse)
{
return; // TODO move?
}
var linMag = Mathf.Clamp(Mathf.Abs(data.Velocity.GetValueOrDefault() - ActualLinVel), 0f, 1f);
ADAccelInput = ActualLinVel < data.Velocity.GetValueOrDefault() ? linMag : -linMag;
ADSteerInput = -Mathf.Clamp(data.SteerAngularVelocity.GetValueOrDefault() * 0.5f, -1f, 1f);
}
}
else if (data.SteerRate.HasValue) // apollo
{
if (double.IsInfinity(data.Acceleration.GetValueOrDefault()) || double.IsInfinity(data.Breaking.GetValueOrDefault()) ||
double.IsNaN(data.Acceleration.GetValueOrDefault()) || double.IsNaN(data.Breaking.GetValueOrDefault()))
{
return;
}
controlType = ControlType.Apollo;
var timeStamp = data.TimeStampSec.GetValueOrDefault();
var dt = (float)(timeStamp - LastTimeStamp);
LastTimeStamp = timeStamp;
Debug.Assert(data.Acceleration.GetValueOrDefault() >= 0 && data.Acceleration.GetValueOrDefault() <= 1);
Debug.Assert(data.Breaking.GetValueOrDefault() >= 0 && data.Breaking.GetValueOrDefault() <= 1);
var linearAccel = AccelerationInputCurve.Evaluate(data.Acceleration.GetValueOrDefault()) - BrakeInputCurve.Evaluate(data.Breaking.GetValueOrDefault());
var steeringTarget = -data.SteerTarget.GetValueOrDefault();
var steeringAngle = Controller.SteerInput;
var sgn = Mathf.Sign(steeringTarget - steeringAngle);
var steeringRate = data.SteerRate.GetValueOrDefault() * sgn;
steeringAngle += steeringRate * dt;
if (sgn != steeringTarget - steeringAngle) // to prevent oversteering
{
steeringAngle = steeringTarget;
}
ADSteerInput = steeringAngle;
float inputAccel = (float)data.Acceleration;
float currentSpeed = RigidBody.velocity.magnitude;;
var throttle = H2VechicleControl_.ConvertToThrottle(currentSpeed, inputAccel, ImuSensor.currentAccel);
if (inputAccel > 0)
{
ADAccelInput = AccelerationInputCurve.Evaluate(throttle);
}
else
{
ADAccelInput = -BrakeInputCurve.Evaluate(H2VechicleControl_.ConvertToBrake(inputAccel));
}
if (data.CurrentGear == GearPosition.Reverse)
{
Dynamics.ShiftReverseAutoGearBox();
}
else if (data.CurrentGear == GearPosition.Drive)
{
Dynamics.ShiftFirstGear();
}
}
else if (data.SteerInput.HasValue) // lgsvl
{
controlType = ControlType.LGSVL;
ADSteerInput = data.SteerInput.GetValueOrDefault();
}
else
{
controlType = ControlType.None;
}
});
}