236 lines
13 KiB
C#
236 lines
13 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Globalization;
|
|
using System.Text;
|
|
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 CreateCurveReport(TrajectoryObservationCharts charts,
|
|
EmTrajectory trajectory)
|
|
{
|
|
if (charts == null || trajectory == null)
|
|
return new TrajectoryObservationDiagnostic("curve report unavailable");
|
|
|
|
IReadOnlyList<EmTrajectoryPoint> points = trajectory.Points;
|
|
var text = new StringBuilder();
|
|
text.AppendLine("LS_CURVE");
|
|
text.AppendLine("index,referenceS(m),lateralOffset(m)");
|
|
for (int index = 0; index < charts.LsSamples.Count; index++)
|
|
{
|
|
TrajectoryObservationLsSample sample = charts.LsSamples[index];
|
|
text.AppendLine(index.ToString(CultureInfo.InvariantCulture) + "," +
|
|
sample.PathS.ToString("R", CultureInfo.InvariantCulture) + "," +
|
|
sample.LateralOffset.ToString("R", CultureInfo.InvariantCulture));
|
|
}
|
|
|
|
text.AppendLine("ST_CURVE");
|
|
text.AppendLine("index,time(s),pathS(m)");
|
|
for (int index = 0; index < charts.StSamples.Count; index++)
|
|
{
|
|
TrajectoryObservationStSample sample = charts.StSamples[index];
|
|
text.AppendLine(index.ToString(CultureInfo.InvariantCulture) + "," +
|
|
sample.TimeFromStart.ToString("R", CultureInfo.InvariantCulture) + "," +
|
|
sample.PathS.ToString("R", CultureInfo.InvariantCulture));
|
|
}
|
|
|
|
text.AppendLine("SPEED_CURVE");
|
|
text.AppendLine("index,time(s),signedSpeed(m/s)");
|
|
for (int index = 0; index < charts.SpeedSamples.Count; index++)
|
|
{
|
|
TrajectoryObservationSpeedSample sample = charts.SpeedSamples[index];
|
|
text.AppendLine(index.ToString(CultureInfo.InvariantCulture) + "," +
|
|
sample.TimeFromStart.ToString("R", CultureInfo.InvariantCulture) + "," +
|
|
sample.SignedLongitudinalVelocity.ToString("R", CultureInfo.InvariantCulture));
|
|
}
|
|
|
|
text.AppendLine("ACCELERATION_CURVE");
|
|
text.AppendLine("index,time(s),acceleration(m/s2)");
|
|
for (int index = 0; index < points.Count; index++)
|
|
{
|
|
text.AppendLine(index.ToString(CultureInfo.InvariantCulture) + "," +
|
|
points[index].TimeFromStart.ToString("R", CultureInfo.InvariantCulture) + "," +
|
|
points[index].LongitudinalAcceleration.ToString("R", CultureInfo.InvariantCulture));
|
|
}
|
|
|
|
text.AppendLine("JERK_CURVE");
|
|
text.AppendLine("index,time(s),jerk(m/s3)");
|
|
for (int index = 0; index + 1 < points.Count; index++)
|
|
{
|
|
text.AppendLine(index.ToString(CultureInfo.InvariantCulture) + "," +
|
|
points[index].TimeFromStart.ToString("R", CultureInfo.InvariantCulture) + "," +
|
|
points[index].LongitudinalJerk.ToString("R", CultureInfo.InvariantCulture));
|
|
}
|
|
|
|
return new TrajectoryObservationDiagnostic(text.ToString());
|
|
}
|
|
|
|
public static TrajectoryObservationDiagnostic CreateConfiguration(EmPlannerConfiguration configuration)
|
|
{
|
|
if (configuration == null || configuration.Scheduling == null || configuration.Solver == null ||
|
|
configuration.Longitudinal == null || configuration.Validation == 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);
|
|
LongitudinalConfiguration longitudinal = configuration.Longitudinal;
|
|
if (!JerkLimitedStoppingMath.TryCalculate(longitudinal.MaximumForwardSpeedMetersPerSecond,
|
|
longitudinal.MaximumAccelerationMetersPerSecondSquared,
|
|
longitudinal.MaximumDecelerationMetersPerSecondSquared,
|
|
longitudinal.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile forwardStop, out _) ||
|
|
!JerkLimitedStoppingMath.TryCalculate(longitudinal.MaximumReverseSpeedMetersPerSecond,
|
|
longitudinal.MaximumAccelerationMetersPerSecondSquared,
|
|
longitudinal.MaximumDecelerationMetersPerSecondSquared,
|
|
longitudinal.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile reverseStop, out _))
|
|
{
|
|
throw new ArgumentException("The configuration cannot construct the jerk-limited stopping diagnostic.",
|
|
nameof(configuration));
|
|
}
|
|
JerkLimitedStoppingProfile worstStop = forwardStop.DistanceMeters >= reverseStop.DistanceMeters
|
|
? forwardStop
|
|
: reverseStop;
|
|
double maximumSpeed = Math.Max(longitudinal.MaximumForwardSpeedMetersPerSecond,
|
|
longitudinal.MaximumReverseSpeedMetersPerSecond);
|
|
double requiredDistanceHorizon = worstStop.DistanceMeters +
|
|
maximumSpeed * configuration.Scheduling.ReplanPeriodSeconds;
|
|
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\n" +
|
|
"kinematicTolerance=" + Format(configuration.Validation.KinematicTolerance, "G17") + "\n" +
|
|
"stopSpeedToleranceMetersPerSecond=" + Format(longitudinal.StopSpeedToleranceMetersPerSecond, "G17") + "\n" +
|
|
"terminalPositionToleranceMeters=" + Format(configuration.Validation.TerminalPositionToleranceMeters, "G17") + "\n" +
|
|
"terminalYawToleranceRadians=" + Format(configuration.Validation.TerminalYawToleranceRadians, "G17") + "\n" +
|
|
"maximumOptimizationTimeStepSeconds=" + Format(configuration.Scheduling.MaximumOptimizationTimeStepSeconds, "F3") + "\n" +
|
|
"maximumOptimizationSpatialStepMeters=" + Format(configuration.Scheduling.MaximumOptimizationSpatialStepMeters, "F3") + "\n" +
|
|
"maximumOptimizationKnotCount=" + configuration.Scheduling.MaximumOptimizationKnotCount.ToString(CultureInfo.InvariantCulture) + "\n" +
|
|
"maximumPublishedSampleCount=" + configuration.Scheduling.MaximumPublishedSampleCount.ToString(CultureInfo.InvariantCulture) + "\n" +
|
|
"maximumForwardSpeedMetersPerSecond=" + Format(longitudinal.MaximumForwardSpeedMetersPerSecond, "F3") + "\n" +
|
|
"maximumReverseSpeedMetersPerSecond=" + Format(longitudinal.MaximumReverseSpeedMetersPerSecond, "F3") + "\n" +
|
|
"maximumAccelerationMetersPerSecondSquared=" + Format(longitudinal.MaximumAccelerationMetersPerSecondSquared, "F3") + "\n" +
|
|
"maximumDecelerationMetersPerSecondSquared=" + Format(longitudinal.MaximumDecelerationMetersPerSecondSquared, "F3") + "\n" +
|
|
"maximumJerkMetersPerSecondCubed=" + Format(longitudinal.MaximumJerkMetersPerSecondCubed, "F3") + "\n" +
|
|
"maximumJerkLimitedStopDistance=" + Format(worstStop.DistanceMeters, "F3") + "m\n" +
|
|
"maximumJerkLimitedStopDuration=" + Format(worstStop.DurationSeconds, "F3") + "s\n" +
|
|
"requiredDistanceHorizon=" + Format(requiredDistanceHorizon, "F3") + "m");
|
|
}
|
|
|
|
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;
|
|
if (!latestCycle.Published || latestCycle.Result.Trajectory == null)
|
|
{
|
|
string missingFailureSummary = CreateMissingFailureSummary(latestCycle.Result.FailureReason);
|
|
if (!string.IsNullOrEmpty(missingFailureSummary))
|
|
text += "\n" + missingFailureSummary;
|
|
}
|
|
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\n" +
|
|
"longitudinalMode=" + trajectory.Metadata.LongitudinalMode + "\n" +
|
|
"terminalSpeed=" + Format(last.SignedLongitudinalVelocity, "F3") + "m/s\n" +
|
|
"terminalAcceleration=" + Format(last.LongitudinalAcceleration, "F3") + "m/s2";
|
|
}
|
|
|
|
private static string CreateMissingFailureSummary(string failureReason)
|
|
{
|
|
string reason = failureReason ?? string.Empty;
|
|
var missing = new List<string>();
|
|
AddMissingFailureField(missing, reason, "longitudinalMode=");
|
|
AddMissingFailureField(missing, reason, "remainingToBoundary=");
|
|
AddMissingFailureField(missing, reason, "minimumStoppingDistance=");
|
|
AddMissingFailureField(missing, reason, "minimumStoppingDuration=");
|
|
AddMissingFailureField(missing, reason, "maximumStoppedReachableDistance=");
|
|
return string.Join("\n", missing);
|
|
}
|
|
|
|
private static void AddMissingFailureField(ICollection<string> missing, string failureReason, string field)
|
|
{
|
|
if (failureReason.IndexOf(field, StringComparison.Ordinal) < 0)
|
|
missing.Add(field + "unavailable");
|
|
}
|
|
|
|
private static bool IsPositiveFinite(double value)
|
|
{
|
|
return !double.IsNaN(value) && !double.IsInfinity(value) && value > 0d;
|
|
}
|
|
}
|