112 lines
5.2 KiB
C#
112 lines
5.2 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Globalization;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
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 (latestCycle.Published && latestCycle.Result.Trajectory != null)
|
|
text += "\n" + CreateTrajectorySummary(latestCycle.Result.Trajectory);
|
|
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);
|
|
}
|
|
|
|
private static string CreateTrajectorySummary(EmTrajectory trajectory)
|
|
{
|
|
IReadOnlyList<EmTrajectoryPoint> points = trajectory.Points;
|
|
double maximumSpeed = 0d;
|
|
double maximumAcceleration = 0d;
|
|
double maximumJerk = 0d;
|
|
double previousAcceleration = 0d;
|
|
bool hasPreviousAcceleration = false;
|
|
double directionSign = trajectory.Metadata.Direction == TravelDirection.Forward ? 1d : -1d;
|
|
for (int index = 0; index < points.Count; index++)
|
|
{
|
|
EmTrajectoryPoint point = points[index];
|
|
maximumSpeed = Math.Max(maximumSpeed, Math.Abs(point.SignedLongitudinalVelocity));
|
|
if (index == 0) continue;
|
|
|
|
EmTrajectoryPoint previous = points[index - 1];
|
|
double dt = point.TimeFromStart - previous.TimeFromStart;
|
|
if (!IsPositiveFinite(dt)) continue;
|
|
double acceleration = (directionSign * point.SignedLongitudinalVelocity -
|
|
directionSign * previous.SignedLongitudinalVelocity) / dt;
|
|
maximumAcceleration = Math.Max(maximumAcceleration, Math.Abs(acceleration));
|
|
if (hasPreviousAcceleration)
|
|
maximumJerk = Math.Max(maximumJerk, Math.Abs((acceleration - previousAcceleration) / dt));
|
|
previousAcceleration = acceleration;
|
|
hasPreviousAcceleration = true;
|
|
}
|
|
|
|
EmTrajectoryPoint first = points[0];
|
|
EmTrajectoryPoint last = points[points.Count - 1];
|
|
return "trajectory summary:\n" +
|
|
"trajectoryId=" + trajectory.Metadata.TrajectoryId + ", points=" +
|
|
points.Count.ToString(CultureInfo.InvariantCulture) + ", duration=" +
|
|
Format(last.TimeFromStart, "F3") + "s, pathLength=" +
|
|
Format(last.PathS - first.PathS, "F3") + "m\n" +
|
|
"maxSpeed=" + Format(maximumSpeed, "F3") + "m/s, maxAcceleration=" +
|
|
Format(maximumAcceleration, "F3") + "m/s2, maxJerk=" +
|
|
Format(maximumJerk, "F3") + "m/s3";
|
|
}
|
|
|
|
private static bool IsPositiveFinite(double value)
|
|
{
|
|
return !double.IsNaN(value) && !double.IsInfinity(value) && value > 0d;
|
|
}
|
|
}
|