refactoring distance driving cycle: ignore slope changes in cycle when limiting simulation distance

VectoCore/Models/SimulationComponent/Impl/DistanceBasedDrivingCycle.cs

@@ -111,25 +111,24 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
 				return new ResponseCycleFinished();
 			}
 
-			if ((PreviousState.Distance + ds).IsGreater(CycleIntervalIterator.RightSample.Distance)) {
-				// only drive until next sample point in cycle
-				Log.Debug("Limiting distance to next sample point {0}",
-					CycleIntervalIterator.RightSample.Distance - PreviousState.Distance);
-				return new ResponseDrivingCycleDistanceExceeded {
-					Source = this,
-					MaxDistance = CycleIntervalIterator.RightSample.Distance - PreviousState.Distance
-				};
+			var nextSpeedChange = GetSpeedChangeWithinSimulationInterval(ds);
+			if (nextSpeedChange == null || ds.IsSmallerOrEqual(nextSpeedChange - PreviousState.Distance)) {
+				return DriveDistance(absTime, ds);
 			}
-
-
-			return DriveDistance(absTime, ds);
+			// only drive until next sample point in cycle with speed change
+			Log.Debug("Limiting distance to next sample point {0}",
+				CycleIntervalIterator.RightSample.Distance - PreviousState.Distance);
+			return new ResponseDrivingCycleDistanceExceeded {
+				Source = this,
+				MaxDistance = nextSpeedChange - PreviousState.Distance
+			};
 		}
 
 		private IResponse DriveTimeInterval(Second absTime, Second dt)
 		{
 			CurrentState.AbsTime = PreviousState.AbsTime + dt;
 			CurrentState.WaitTime = PreviousState.WaitTime + dt;
-			CurrentState.Gradient = ComputeGradient();
+			CurrentState.Gradient = ComputeGradient(0.SI<Meter>());
 			CurrentState.VehicleTargetSpeed = CycleIntervalIterator.LeftSample.VehicleTargetSpeed;
 
 			return NextComponent.Request(absTime, dt, CycleIntervalIterator.LeftSample.VehicleTargetSpeed, CurrentState.Gradient);
@@ -137,42 +136,62 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
 
 		private IResponse DriveDistance(Second absTime, Meter ds)
 		{
-			if (!CurrentState.RequestToNextSamplePointDone &&
-				(CycleIntervalIterator.RightSample.Distance - PreviousState.Distance) <
-				Constants.SimulationSettings.BrakeNextTargetDistance) {
+			var nextSpeedChanges = LookAhead(Constants.SimulationSettings.BrakeNextTargetDistance);
+			if (nextSpeedChanges.Count > 0 && !CurrentState.RequestToNextSamplePointDone) {
 				CurrentState.RequestToNextSamplePointDone = true;
 				Log.Debug("current distance is close to the next speed change: {0}",
-					CycleIntervalIterator.RightSample.Distance - PreviousState.Distance);
+					nextSpeedChanges.First().Distance - PreviousState.Distance);
 				return new ResponseDrivingCycleDistanceExceeded {
 					Source = this,
 					MaxDistance = Constants.SimulationSettings.BrakeNextTargetDistance
 				};
 			}
+
 			CurrentState.Distance = PreviousState.Distance + ds;
 			CurrentState.VehicleTargetSpeed = CycleIntervalIterator.LeftSample.VehicleTargetSpeed;
-			CurrentState.Gradient = ComputeGradient();
+			CurrentState.Gradient = ComputeGradient(ds);
 
 			return NextComponent.Request(absTime, ds, CurrentState.VehicleTargetSpeed, CurrentState.Gradient);
 		}
 
-		private Radian ComputeGradient()
+		private Radian ComputeGradient(Meter ds)
 		{
 			var leftSamplePoint = CycleIntervalIterator.LeftSample;
-			var rightSamplePoint = CycleIntervalIterator.RightSample;
 
-			var gradient = leftSamplePoint.RoadGradient;
+			var cycleIterator = CycleIntervalIterator.Clone();
+			while (cycleIterator.RightSample.Distance < PreviousState.Distance + ds && !cycleIterator.LastEntry) {
+				cycleIterator.MoveNext();
+			}
+			var rightSamplePoint = cycleIterator.RightSample;
 
-			if (!leftSamplePoint.Distance.IsEqual(rightSamplePoint.Distance)) {
-				CurrentState.Altitude = VectoMath.Interpolate(leftSamplePoint.Distance, rightSamplePoint.Distance,
-					leftSamplePoint.Altitude, rightSamplePoint.Altitude, CurrentState.Distance);
+			var gradient = leftSamplePoint.RoadGradient;
 
-				gradient = VectoMath.InclinationToAngle(((CurrentState.Altitude - PreviousState.Altitude) /
-														(CurrentState.Distance - PreviousState.Distance)).Value());
+			if (leftSamplePoint.Distance.IsEqual(rightSamplePoint.Distance)) {
+				return gradient;
 			}
+
+			CurrentState.Altitude = VectoMath.Interpolate(leftSamplePoint.Distance, rightSamplePoint.Distance,
+				leftSamplePoint.Altitude, rightSamplePoint.Altitude, PreviousState.Distance + ds);
+
+			gradient = VectoMath.InclinationToAngle(((CurrentState.Altitude - PreviousState.Altitude) /
+													(ds)).Value());
 			//return 0.SI<Radian>();
 			return gradient;
 		}
 
+		private Meter GetSpeedChangeWithinSimulationInterval(Meter ds)
+		{
+			var leftSamplePoint = CycleIntervalIterator.LeftSample;
+			var cycleIterator = CycleIntervalIterator.Clone();
+
+			do {
+				if (!leftSamplePoint.VehicleTargetSpeed.IsEqual(cycleIterator.RightSample.VehicleTargetSpeed)) {
+					return cycleIterator.RightSample.Distance;
+				}
+			} while (cycleIterator.RightSample.Distance < PreviousState.Distance + ds && cycleIterator.MoveNext());
+			return null;
+		}
+
 		IResponse ISimulationOutPort.Request(Second absTime, Second dt)
 		{
 			throw new NotImplementedException();
@@ -239,10 +258,17 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
 			}
 
 			// separately test for equality and greater than to have tolerance for equality comparison
-			if (CycleIntervalIterator.LeftSample.StoppingTime.IsEqual(0) &&
-				CurrentState.Distance.IsGreaterOrEqual(CycleIntervalIterator.RightSample.Distance)) {
-				// we have reached the end of the current interval in the cycle, move on...
-				CycleIntervalIterator.MoveNext();
+			if (CycleIntervalIterator.LeftSample.StoppingTime.IsEqual(0)) {
+				while (CurrentState.Distance.IsGreaterOrEqual(CycleIntervalIterator.RightSample.Distance) &&
+						!CycleIntervalIterator.LastEntry) {
+					// we have reached the end of the current interval in the cycle, move on...
+					CycleIntervalIterator.MoveNext();
+				}
+			} else {
+				if (CycleIntervalIterator.LeftSample.StoppingTime.IsEqual(PreviousState.WaitTime)) {
+					// we needed to stop at the current interval in the cycle and have already waited enough time, move on..
+					CycleIntervalIterator.MoveNext();
+				}
 			}
 		}
 
@@ -262,6 +288,10 @@ namespace TUGraz.VectoCore.Models.SimulationComponent.Impl
 				retVal.Add(cycleIterator.RightSample);
 				velocity = cycleIterator.RightSample.VehicleTargetSpeed;
 			} while (cycleIterator.MoveNext() && cycleIterator.RightSample.Distance < PreviousState.Distance + lookaheadDistance);
+			if (retVal.Count > 0) {
+				retVal = retVal.Where(x => x.Distance <= PreviousState.Distance + lookaheadDistance).ToList();
+				retVal.Sort((x, y) => x.Distance.CompareTo(y.Distance));
+			}
 			return retVal;
 		}
 

VectoCoreTest/Models/SimulationComponent/DistanceBasedDrivingCycleTest.cs

@@ -6,6 +6,7 @@ using TUGraz.VectoCore.Models.Connector.Ports.Impl;
 using TUGraz.VectoCore.Models.Simulation.Impl;
 using TUGraz.VectoCore.Models.SimulationComponent.Data;
 using TUGraz.VectoCore.Models.SimulationComponent.Impl;
+using TUGraz.VectoCore.Tests.Integration;
 using TUGraz.VectoCore.Tests.Utils;
 using TUGraz.VectoCore.Utils;
 
@@ -18,6 +19,96 @@ namespace TUGraz.VectoCore.Tests.Models.SimulationComponent
 
 		public const double Tolerance = 0.0001;
 
+
+		[TestMethod]
+		public void TestLimitRequst()
+		{
+			var data = new string[] {
+				// <s>,<v>,<grad>,<stop>
+				"  0,  20,  0,     0",
+				"  1,  20, -0.1,   0",
+				"  2,  20, -0.3,   0",
+				" 10,  40, -0.3,   0",
+				" 20,  30, -0.1,   0"
+			};
+			var cycleData = SimpleDrivingCycles.CreateCycleData(data);
+			var container = new VehicleContainer();
+			var cycle = new DistanceBasedDrivingCycle(container, cycleData);
+
+			var gbx = new MockGearbox(container);
+
+			var driver = new MockDriver(container);
+			cycle.InPort().Connect(driver.OutPort());
+
+			cycle.OutPort().Initialize();
+
+			// just in test mock driver
+			driver.VehicleStopped = false;
+
+			var absTime = 0.SI<Second>();
+
+			// a request up to 10m succeeds, no speed change for the next 10m
+
+			var response = cycle.OutPort().Request(absTime, 0.3.SI<Meter>());
+			Assert.IsInstanceOfType(response, typeof(ResponseSuccess));
+
+			response = cycle.OutPort().Request(absTime, 1.SI<Meter>());
+			Assert.IsInstanceOfType(response, typeof(ResponseSuccess));
+
+			response = cycle.OutPort().Request(absTime, 1.3.SI<Meter>());
+			Assert.IsInstanceOfType(response, typeof(ResponseSuccess));
+
+			response = cycle.OutPort().Request(absTime, 2.7.SI<Meter>());
+			Assert.IsInstanceOfType(response, typeof(ResponseSuccess));
+
+			response = cycle.OutPort().Request(absTime, 3.5.SI<Meter>());
+			Assert.IsInstanceOfType(response, typeof(ResponseSuccess));
+
+			// a request with 12m exceeds the speed change at 10m -> maxDistance == 10m
+
+			response = cycle.OutPort().Request(absTime, 12.SI<Meter>());
+			Assert.IsInstanceOfType(response, typeof(ResponseDrivingCycleDistanceExceeded));
+			Assert.AreEqual(10, ((ResponseDrivingCycleDistanceExceeded)response).MaxDistance.Value());
+
+			response = cycle.OutPort().Request(absTime, 10.SI<Meter>());
+			Assert.IsInstanceOfType(response, typeof(ResponseSuccess));
+
+			// drive 10m
+			container.CommitSimulationStep(absTime, response.SimulationInterval);
+			absTime += response.SimulationInterval;
+
+			// - - - - - - - - 
+			// request with 8m succeeds
+
+			response = cycle.OutPort().Request(absTime, 8.SI<Meter>());
+			Assert.IsInstanceOfType(response, typeof(ResponseSuccess));
+
+			container.CommitSimulationStep(absTime, response.SimulationInterval);
+			absTime += response.SimulationInterval;
+
+			// - - - - - - - - 
+			// request with 3m more -> distance exceeded. maxDistance == 2m (approach next speed change, we are within 5m radius)
+
+			response = cycle.OutPort().Request(absTime, 3.SI<Meter>());
+			Assert.IsInstanceOfType(response, typeof(ResponseDrivingCycleDistanceExceeded));
+			Assert.AreEqual(2, ((ResponseDrivingCycleDistanceExceeded)response).MaxDistance.Value());
+
+			// - - - - - - - - 
+			// request with 1m (18 -> 19m) => response exceeded, drive up to next sample point (at least 5m)
+			response = cycle.OutPort().Request(absTime, 1.SI<Meter>());
+			Assert.IsInstanceOfType(response, typeof(ResponseDrivingCycleDistanceExceeded));
+			Assert.AreEqual(5, ((ResponseDrivingCycleDistanceExceeded)response).MaxDistance.Value());
+
+			// next request with 5m, as suggested => distance exceeded. maxDistance == 2m (next speed change)....
+			response = cycle.OutPort().Request(absTime, ((ResponseDrivingCycleDistanceExceeded)response).MaxDistance);
+			Assert.IsInstanceOfType(response, typeof(ResponseDrivingCycleDistanceExceeded));
+			Assert.AreEqual(2, ((ResponseDrivingCycleDistanceExceeded)response).MaxDistance.Value());
+
+			// ok
+			response = cycle.OutPort().Request(absTime, ((ResponseDrivingCycleDistanceExceeded)response).MaxDistance);
+			Assert.IsInstanceOfType(response, typeof(ResponseSuccess));
+		}
+
 		[TestMethod]
 		public void TestDistanceRequest()
 		{
@@ -36,7 +127,7 @@ namespace TUGraz.VectoCore.Tests.Models.SimulationComponent
 			// just in test mock driver
 			driver.VehicleStopped = false;
 
-			var startDistance = cycleData.Entries.First().Distance.Value();
+			var startDistance = container.CycleStartDistance.Value();
 			var absTime = 0.SI<Second>();
 
 			// waiting time of 40 seconds is split up to 3 steps: 0.5, 39, 0.5
@@ -74,7 +165,7 @@ namespace TUGraz.VectoCore.Tests.Models.SimulationComponent
 
 			container.CommitSimulationStep(absTime, response.SimulationInterval);
 			absTime += response.SimulationInterval;
-			
+
 			response = cycle.OutPort().Request(absTime, 1.SI<Meter>());
 
 			Assert.IsInstanceOfType(response, typeof(ResponseSuccess));