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; } }); }