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