public override void OnBridgeSetup(BridgeInstance bridge)
{
var subscriber = BridgeMessageDispatcher.Instance.GetSynchronousSubscriber <VehicleControlData>(data =>
{
if (!IsControlReceived)
{
IsControlReceived = true;
if (ApiManager.Instance != null)
{
var jsonData = new JSONObject();
ApiManager.Instance.AddCustom(transform.parent.gameObject, "checkControl", jsonData);
}
}
controlData = data;
LastControlUpdate = SimulatorManager.Instance.CurrentTime;
if (double.IsInfinity(data.Acceleration.GetValueOrDefault()) || double.IsInfinity(data.Braking.GetValueOrDefault()) ||
double.IsNaN(data.Acceleration.GetValueOrDefault()) || double.IsNaN(data.Braking.GetValueOrDefault()))
{
return;
}
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.Braking.GetValueOrDefault() >= 0 && data.Braking.GetValueOrDefault() <= 1);
var linearAccel = AccelerationInputCurve.Evaluate(data.Acceleration.GetValueOrDefault()) - BrakeInputCurve.Evaluate(data.Braking.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) // prevent oversteering
{
steeringAngle = steeringTarget;
}
ADSteerInput = steeringAngle;
ADAccelInput = linearAccel;
if (data.CurrentGear == GearPosition.Reverse)
{
Dynamics.ShiftReverseAutoGearBox();
}
else if (data.CurrentGear == GearPosition.Drive)
{
Dynamics.ShiftFirstGear();
}
});
bridge.AddSubscriber(Topic, subscriber);
}
public override void OnBridgeSetup(BridgeInstance bridge)
{
if (bridge.Plugin.GetBridgeNameAttribute().Type == "ROS")
{
bridge.AddSubscriber <VehicleControlData>(Topic, data =>
{
if (Time.timeScale == 0f)
{
return;
}
controlData = data;
LastControlUpdate = SimulatorManager.Instance.CurrentTime;
float wheelAngle = data.SteerAngle.GetValueOrDefault();
wheelAngle = UnityEngine.Mathf.Clamp(wheelAngle, -Dynamics.MaxSteeringAngle, Dynamics.MaxSteeringAngle);
var k = (float)(wheelAngle + Dynamics.MaxSteeringAngle) / (Dynamics.MaxSteeringAngle * 2);
ADSteerInput = UnityEngine.Mathf.Lerp(-1f, 1f, k);
ADAccelInput = data.Acceleration.GetValueOrDefault() - data.Braking.GetValueOrDefault();
});
}
else if (bridge.Plugin.GetBridgeNameAttribute().Type == "ROS2")
{
bridge.AddSubscriber <VehicleControlData>(Topic, data =>
{
if (Time.timeScale == 0f)
{
return;
}
controlData = data;
LastControlUpdate = SimulatorManager.Instance.CurrentTime;
float wheelAngle = data.SteerAngle.GetValueOrDefault();
ADSteerInput = wheelAngle;
ADAccelInput = data.Acceleration.GetValueOrDefault() - data.Braking.GetValueOrDefault();
if (data.TargetGear == GearPosition.Reverse)
{
Dynamics.ShiftReverseAutoGearBox();
}
else if (data.TargetGear == GearPosition.Drive)
{
Dynamics.ShiftFirstGear();
}
});
}
}
private void Update()
{
var currentSpeed = Dynamics.Velocity.magnitude;
if (seq == null)
{
stage = Stage.End;
}
if (stage == Stage.End)
{
return;
}
if (firstRun && seq != null)
{
state = States[seq.Value];
duration = States[seq.Value].duration;
state.throttle = state.throttle * 0.01f;
state.brakes = state.brakes * 0.01f;
state.steering = state.steering * 0.01f;
upperBound = States[seq.Value].max_velocity * 0.9f;
lowerBound = States[seq.Value].min_velocity * 1.1f;
maxVelocity = States[seq.Value].max_velocity;
minVelocity = States[seq.Value].min_velocity;
AccelInput = 0f;
SteerInput = 0f;
if (States[seq.Value].gear == "forward")
{
whatToTest = (States[seq.Value].throttle > 0) ? WhatToTest.ForwardThrottle
: (States[seq.Value].brakes > 0) ? WhatToTest.ForwardBrake : WhatToTest.None;
}
else if (States[seq.Value].gear == "reverse")
{
whatToTest = (States[seq.Value].throttle > 0) ? WhatToTest.ReverseThrottle
: (States[seq.Value].brakes > 0) ? WhatToTest.ReverseBrake : WhatToTest.None;
}
firstRun = false;
}
if (stage == Stage.Init)
{
if (whatToTest == WhatToTest.ForwardThrottle || whatToTest == WhatToTest.ForwardBrake)
{
if (whatToTest == WhatToTest.ForwardThrottle && velocityState == VelocityState.Decreasing)
{
AccelInput = -1.0f;
if (currentSpeed < lowerBound)
{
stage = Stage.Apply;
StartTime = Time.time;
velocityState = VelocityState.Stay;
AccelInput = state.throttle - state.brakes;
idxApply = 0;
}
}
else if (whatToTest == WhatToTest.ForwardThrottle && velocityState == VelocityState.Increasing)
{
AccelInput = 1.0f;
if (currentSpeed > lowerBound && currentSpeed < upperBound)
{
stage = Stage.Apply;
StartTime = Time.time;
velocityState = VelocityState.Stay;
AccelInput = state.throttle - state.brakes;
idxApply = 0;
}
}
if (whatToTest == WhatToTest.ForwardBrake && velocityState == VelocityState.Decreasing)
{
AccelInput = -1.0f;
if (currentSpeed < upperBound && currentSpeed > lowerBound)
{
stage = Stage.Apply;
StartTime = Time.time;
velocityState = VelocityState.Stay;
AccelInput = state.throttle - state.brakes;
idxApply = 0;
}
}
else if (whatToTest == WhatToTest.ForwardBrake && velocityState == VelocityState.Increasing)
{
AccelInput = 1.0f;
if (currentSpeed > upperBound)
{
stage = Stage.Apply;
StartTime = Time.time;
velocityState = VelocityState.Stay;
AccelInput = state.throttle - state.brakes;
idxApply = 0;
}
}
}
else if (whatToTest == WhatToTest.ReverseThrottle || whatToTest == WhatToTest.ReverseBrake)
{
if (Dynamics.Reverse == true)
{
if (whatToTest == WhatToTest.ReverseThrottle || whatToTest == WhatToTest.ReverseBrake)
{
if (whatToTest == WhatToTest.ReverseThrottle && velocityState == VelocityState.Decreasing)
{
AccelInput = -1.0f;
if (currentSpeed < lowerBound)
{
stage = Stage.Apply;
StartTime = Time.time;
velocityState = VelocityState.Stay;
AccelInput = state.throttle - state.brakes;
idxApply = 0;
}
}
else if (whatToTest == WhatToTest.ReverseThrottle && velocityState == VelocityState.Increasing)
{
AccelInput = 1.0f;
if (currentSpeed > lowerBound && currentSpeed < upperBound)
{
stage = Stage.Apply;
StartTime = Time.time;
velocityState = VelocityState.Stay;
AccelInput = state.throttle - state.brakes;
idxApply = 0;
}
}
if (whatToTest == WhatToTest.ReverseBrake && velocityState == VelocityState.Decreasing)
{
AccelInput = -1.0f;
if (currentSpeed < upperBound && currentSpeed > lowerBound)
{
stage = Stage.Apply;
StartTime = Time.time;
velocityState = VelocityState.Stay;
AccelInput = state.throttle - state.brakes;
idxApply = 0;
}
}
else if (whatToTest == WhatToTest.ReverseBrake && velocityState == VelocityState.Increasing)
{
AccelInput = 1.0f;
if (currentSpeed > upperBound)
{
stage = Stage.Apply;
StartTime = Time.time;
velocityState = VelocityState.Stay;
AccelInput = state.throttle - state.brakes;
idxApply = 0;
}
}
}
}
else
{
Dynamics.ShiftReverseAutoGearBox();
}
}
}
else if (stage == Stage.Apply)
{
idxApply++;
ElapsedTime = Time.time - StartTime;
AccelInput = state.throttle - state.brakes;
SteerInput = -state.steering;
if (ElapsedTime > duration)
{
stage = Stage.DeInit;
ElapsedTime = 0f;
}
else if (currentSpeed > maxVelocity || currentSpeed < minVelocity)
{
stage = Stage.Init;
duration = duration - ElapsedTime;
ElapsedTime = 0f;
if (currentSpeed > maxVelocity)
{
velocityState = VelocityState.Decreasing;
AccelInput = 0f;
SteerInput = 0f;
}
else if (currentSpeed < minVelocity)
{
velocityState = VelocityState.Increasing;
AccelInput = 0f;
SteerInput = 0f;
}
}
}
else if (stage == Stage.DeInit)
{
AccelInput = -0.8f;
SteerInput = 0f;
if (currentSpeed < 0.5f)
{
stage = Stage.Init;
velocityState = VelocityState.Increasing;
seq = (seq < States.Count - 1) ? seq + 1 : null;
firstRun = true;
}
}
//// Every 0.1 sec, it prints out debug msg.
//if ((int)((Time.time - StartTime) * 10.0f) % 10 == 0)
//{
// Debug.Log($"seq: {seq.Value}/{states.Count}, idxApply: {idxApply.Value}, stage: {stage.ToString()}, ElapsedTime: " +
// $"{ElapsedTime}, Duration: {duration}, curr_v: " +
// $"{currentSpeed} [max_v: {state.max_velocity}," +
// $" min_v: {state.min_velocity}, upper_v: {upperBound}, lower_v: {lowerBound}] AccelInput: {AccelInput} " +
// $"[throttle: {state.throttle}, brakes: {state.brakes}], " +
// $"SteerInput: {SteerInput} steer: [{state.steering}], gear: " +
// $"{dynamics.CurrentGear} [{state.gear}], whatToTest: {whatToTest}, " +
// $"velocityState: {velocityState}");
//}
}
public override void OnBridgeSetup(IBridge bridge)
{
bridge.AddReader <VehicleControlData>(Topic, data =>
{
controlData = data;
LastControlUpdate = SimulatorManager.Instance.CurrentTime;
if (data.Velocity.HasValue) // autoware
{
controlType = ControlType.AutowareAi;
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;
ADAccelInput = linearAccel;
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 if (data.Acceleration.HasValue)
{
controlType = ControlType.AutowareAuto;
ADAccelInput = data.Acceleration.GetValueOrDefault() - data.Breaking.GetValueOrDefault();
ADSteerInput = data.SteerAngle.GetValueOrDefault();
}
else
{
controlType = ControlType.None;
}
});
}
public override void OnBridgeSetup(BridgeInstance bridge)
{
bridge.AddSubscriber <VehicleStateData>(Topic, data =>
{
if (Time.timeScale == 0f)
{
return;
}
if (data != null)
{
if (StateData.Blinker != data.Blinker)
{
if (data.Blinker == 0)
{
if (Actions.LeftTurnSignal)
{
Actions.LeftTurnSignal = false;
}
if (Actions.RightTurnSignal)
{
Actions.RightTurnSignal = false;
}
if (Actions.HazardLights)
{
Actions.HazardLights = false;
}
}
else if (data.Blinker == 1)
{
Actions.LeftTurnSignal = true;
}
else if (data.Blinker == 2)
{
Actions.RightTurnSignal = true;
}
else if (data.Blinker == 3)
{
Actions.HazardLights = true;
}
}
if (StateData.HeadLight != data.HeadLight)
{
if (data.HeadLight == 0)
{
Actions.CurrentHeadLightState = HeadLightState.OFF;
}
else if (data.HeadLight == 1)
{
Actions.CurrentHeadLightState = HeadLightState.LOW;
}
else if (data.HeadLight == 2)
{
Actions.CurrentHeadLightState = HeadLightState.HIGH;
}
}
if (StateData.Gear != data.Gear)
{
if (data.Gear == (byte)GearPosition.Reverse)
{
Dynamics.ShiftReverseAutoGearBox();
}
else if (data.Gear == (byte)GearPosition.Drive)
{
Dynamics.ShiftFirstGear();
}
}
if (StateData.HandBrake != data.HandBrake)
{
if (data.HandBrake == true)
{
Dynamics.SetHandBrake(true);
}
else
{
Dynamics.SetHandBrake(false);
}
}
StateData = data;
}
});
}