ParkingRobot/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs

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 ||
            configuration.Longitudinal == 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" +
            "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;
    }
}