66 lines
3.0 KiB
C#
66 lines
3.0 KiB
C#
using System;
|
|
using System.Globalization;
|
|
using MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation;
|
|
|
|
public sealed class TrajectoryObservationDiagnostic
|
|
{
|
|
public TrajectoryObservationDiagnostic(string text)
|
|
{
|
|
Text = text ?? string.Empty;
|
|
}
|
|
|
|
public string Text { get; }
|
|
}
|
|
|
|
public static class TrajectoryObservationDiagnostics
|
|
{
|
|
public static TrajectoryObservationDiagnostic CreateConfiguration(EmPlannerConfiguration configuration)
|
|
{
|
|
if (configuration == null || configuration.Scheduling == null || configuration.Solver == null)
|
|
throw new ArgumentNullException(nameof(configuration));
|
|
|
|
double horizon = configuration.Scheduling.TimeHorizonSeconds;
|
|
double outputTimeStep = configuration.Scheduling.OutputTimeStepSeconds;
|
|
int trajectoryKnots = checked((int)Math.Ceiling(horizon / outputTimeStep) + 1);
|
|
return new TrajectoryObservationDiagnostic(
|
|
"planning configuration:\n" +
|
|
"timeHorizon=" + Format(horizon, "F2") + "s\n" +
|
|
"distanceHorizon=" + Format(configuration.Scheduling.DistanceHorizonMeters, "F2") + "m\n" +
|
|
"outputTimeStep=" + Format(outputTimeStep, "F2") + "s\n" +
|
|
"outputFrequency=" + Format(1d / outputTimeStep, "F2") + "Hz\n" +
|
|
"trajectoryKnots=" + trajectoryKnots.ToString(CultureInfo.InvariantCulture) + "\n" +
|
|
"maximumOsqpIterations=" + configuration.Solver.MaximumOsqpIterations.ToString(CultureInfo.InvariantCulture) + "\n" +
|
|
"solverTimeout=" + Format(configuration.Scheduling.SolverTimeoutSeconds, "F2") + "s\n" +
|
|
"replanPeriod=" + Format(configuration.Scheduling.ReplanPeriodSeconds, "F2") + "s");
|
|
}
|
|
|
|
public static TrajectoryObservationDiagnostic Create(PlanningCycleResult latestCycle,
|
|
TimeSpan elapsed, bool planningInFlight, EmTrajectory publishedTrajectory)
|
|
{
|
|
if (latestCycle == null)
|
|
return new TrajectoryObservationDiagnostic(planningInFlight
|
|
? "planning status=pending"
|
|
: "planning status=not-started");
|
|
|
|
string text = "planning cycle=" + latestCycle.Version.ToString(CultureInfo.InvariantCulture) +
|
|
" status=" + latestCycle.Result.Status +
|
|
" published=" + latestCycle.Published +
|
|
" elapsed=" + Math.Max(0d, elapsed.TotalMilliseconds).ToString("F0", CultureInfo.InvariantCulture) +
|
|
"ms";
|
|
if (planningInFlight)
|
|
text += "\nreplan=pending";
|
|
if (publishedTrajectory != null)
|
|
text += "\ntrajectory=" + publishedTrajectory.Metadata.TrajectoryId;
|
|
if (!string.IsNullOrWhiteSpace(latestCycle.Result.FailureReason))
|
|
text += "\nreason=" + latestCycle.Result.FailureReason;
|
|
return new TrajectoryObservationDiagnostic(text);
|
|
}
|
|
|
|
private static string Format(double value, string format)
|
|
{
|
|
return value.ToString(format, CultureInfo.InvariantCulture);
|
|
}
|
|
}
|