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; 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; } }); }
public override void OnBridgeSetup(IBridge bridge) { bridge.AddReader <Detected2DObjectArray>(Topic, data => Detected = data.Data); }
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; } }); }