89 lines
3.6 KiB
C#
89 lines
3.6 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Collections.ObjectModel;
|
|
using System.Threading;
|
|
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
namespace EMPlannerVerificationHost;
|
|
|
|
internal sealed class FakeQpSolver : IQpSolver
|
|
{
|
|
private readonly Queue<QpSolveResult> _results;
|
|
private readonly List<QuadraticProgram> _problems = new List<QuadraticProgram>();
|
|
private readonly List<IReadOnlyList<double>> _warmStarts = new List<IReadOnlyList<double>>();
|
|
|
|
public FakeQpSolver(QpSolveResult result)
|
|
: this(new[] { result })
|
|
{
|
|
}
|
|
|
|
public FakeQpSolver(IEnumerable<QpSolveResult> results)
|
|
{
|
|
if (results == null)
|
|
throw new ArgumentNullException(nameof(results));
|
|
_results = new Queue<QpSolveResult>();
|
|
foreach (QpSolveResult result in results)
|
|
_results.Enqueue(result ?? throw new ArgumentException("Fake solver results cannot contain null values.", nameof(results)));
|
|
if (_results.Count == 0)
|
|
throw new ArgumentException("At least one fake solver result is required.", nameof(results));
|
|
LastWarmStart = Array.Empty<double>();
|
|
}
|
|
|
|
public QuadraticProgram? LastProblem { get; private set; }
|
|
|
|
public QpSolverSettings? LastSettings { get; private set; }
|
|
|
|
public IReadOnlyList<double> LastWarmStart { get; private set; }
|
|
|
|
public IReadOnlyList<QuadraticProgram> Problems => new ReadOnlyCollection<QuadraticProgram>(_problems);
|
|
|
|
public IReadOnlyList<IReadOnlyList<double>> WarmStarts => new ReadOnlyCollection<IReadOnlyList<double>>(_warmStarts);
|
|
|
|
public int SolveCallCount => _problems.Count;
|
|
|
|
public QpSolveResult Solve(QuadraticProgram problem, QpSolverSettings settings, IReadOnlyList<double> warmStart,
|
|
CancellationToken cancellationToken)
|
|
{
|
|
LastProblem = problem ?? throw new ArgumentNullException(nameof(problem));
|
|
LastSettings = settings ?? throw new ArgumentNullException(nameof(settings));
|
|
var copy = new List<double>(warmStart == null ? 0 : warmStart.Count);
|
|
if (warmStart != null)
|
|
{
|
|
for (int index = 0; index < warmStart.Count; index++)
|
|
copy.Add(warmStart[index]);
|
|
}
|
|
LastWarmStart = new ReadOnlyCollection<double>(copy);
|
|
_problems.Add(LastProblem);
|
|
_warmStarts.Add(LastWarmStart);
|
|
if (_results.Count == 0)
|
|
throw new InvalidOperationException("Fake solver was called more often than its scripted result sequence.");
|
|
return _results.Dequeue();
|
|
}
|
|
}
|
|
|
|
internal sealed class CancellingQpSolver : IQpSolver
|
|
{
|
|
private readonly IQpSolver inner;
|
|
private readonly CancellationTokenSource cancellation;
|
|
private readonly int cancelAfterSolveCount;
|
|
private int solveCount;
|
|
|
|
public CancellingQpSolver(IQpSolver inner, CancellationTokenSource cancellation, int cancelAfterSolveCount = 1)
|
|
{
|
|
this.inner = inner ?? throw new ArgumentNullException(nameof(inner));
|
|
this.cancellation = cancellation ?? throw new ArgumentNullException(nameof(cancellation));
|
|
if (cancelAfterSolveCount <= 0) throw new ArgumentOutOfRangeException(nameof(cancelAfterSolveCount));
|
|
this.cancelAfterSolveCount = cancelAfterSolveCount;
|
|
}
|
|
|
|
public QpSolveResult Solve(QuadraticProgram problem, QpSolverSettings settings, IReadOnlyList<double> warmStart,
|
|
CancellationToken cancellationToken)
|
|
{
|
|
QpSolveResult result = inner.Solve(problem, settings, warmStart, cancellationToken);
|
|
solveCount++;
|
|
if (solveCount == cancelAfterSolveCount)
|
|
cancellation.Cancel();
|
|
return result;
|
|
}
|
|
}
|