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 static Entity CreateVehicleTemplate(Vector3 position, Quaternion rotation, VehicleControlData controlData, SensorData sensorData) { return(EntityBuilder.Begin() .AddPositionComponent(position, CommonRequirementSets.PhysicsOnly) .AddMetadataComponent(entityType: SimulationSettings.VehiclePrefabName) .SetPersistence(true) .SetReadAcl(CommonRequirementSets.PhysicsOrVisual) .AddComponent(new Rotation.Data(rotation.ToNativeQuaternion()), CommonRequirementSets.PhysicsOnly) .AddComponent(new VehicleControl.Data(controlData), CommonRequirementSets.PhysicsOnly) .AddComponent(new Sensor.Data(sensorData), CommonRequirementSets.PhysicsOnly) .Build()); }