Code development platform for open source projects from the European Union institutions :large_blue_circle: EU Login authentication by SMS will be completely phased out by mid-2025. To see alternatives please check here

Skip to content
Snippets Groups Projects
Commit 1d8ca489 authored by Markus Quaritsch's avatar Markus Quaritsch
Browse files

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

parent 4a244076
No related branches found
No related tags found
No related merge requests found
......@@ -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;
}
......
......@@ -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));
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment