private void ShiftFirstPerformed(UnityEngine.InputSystem.InputAction.CallbackContext obj)
 {
     dynamics.ShiftFirstGear();
 }
Example #2
0
        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;
                    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
                {
                    controlType = ControlType.None;
                }
            });
        }
Example #3
0
        public override void OnBridgeSetup(IBridge bridge)
        {
            bridge.AddReader <VehicleStateData>(Topic, data =>
            {
                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 = VehicleActions.HeadLightState.OFF;
                        }
                        else if (data.HeadLight == 1)
                        {
                            Actions.CurrentHeadLightState = VehicleActions.HeadLightState.LOW;
                        }
                        else if (data.HeadLight == 2)
                        {
                            Actions.CurrentHeadLightState = VehicleActions.HeadLightState.HIGH;
                        }
                    }
                    if (StateData.Gear != data.Gear)
                    {
                        if (data.Gear == 1)
                        {
                            Dynamics.ShiftReverse();
                        }
                        else if (data.Gear == 0)
                        {
                            Dynamics.ShiftFirstGear();
                        }
                    }
                    if (StateData.HandBrake != data.HandBrake)
                    {
                        if (data.HandBrake == true)
                        {
                            Dynamics.SetHandBrake(true);
                        }
                        else
                        {
                            Dynamics.SetHandBrake(false);
                        }
                    }

                    StateData = data;
                }
            });
        }