ParkingRobot/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs

using System;
using System.Collections.Generic;
using EMPlannerVerificationHost;
using MultiWheelC.TrajectoryPlanning.CoarsePath;

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

internal static class TrajectoryChecks
{
    public static void Run()
    {
        VerifiesForwardFieldsExactTerminalAndHold();
        VerifiesReverseTravelVelocityAndUnwrappedYaw();
        VerifiesPublishedListsAreImmutable();
    }

    private static void VerifiesForwardFieldsExactTerminalAndHold()
    {
        EmTrajectory trajectory = new EmTrajectoryAssembler().Assemble(
            CreatePath(TravelDirection.Forward, 0d, Math.PI / 2d),
            CreateLongitudinalResult(),
            CreateMetadata(TravelDirection.Forward, EmTerminalType.Goal));

        VerifyKinematicFields(trajectory, TravelDirection.Forward, "forward");
        VerifyTerminalAndHold(trajectory, EmBoundaryType.Goal, "forward");
    }

    private static void VerifiesReverseTravelVelocityAndUnwrappedYaw()
    {
        EmTrajectory trajectory = new EmTrajectoryAssembler().Assemble(
            CreatePath(TravelDirection.Reverse, 3.10d, -3.10d),
            CreateLongitudinalResult(),
            CreateMetadata(TravelDirection.Reverse, EmTerminalType.GearSwitch));

        VerifyKinematicFields(trajectory, TravelDirection.Reverse, "reverse");
        VerifyTerminalAndHold(trajectory, EmBoundaryType.GearSwitchApproach, "reverse");
        EmTrajectoryPoint moving = trajectory.Points[1];
        Verification.True(moving.SignedLongitudinalVelocity < 0d, "reverse signed velocity is negative");
        Verification.True(moving.VelocityX * Math.Cos(moving.Yaw) + moving.VelocityY * Math.Sin(moving.Yaw) < 0d,
            "reverse world velocity points opposite the vehicle yaw");
        Verification.True(Math.Abs(Math.Abs(moving.Yaw) - Math.PI) < 0.1d,
            "reverse yaw interpolation unwraps across the pi boundary");
    }

    private static void VerifiesPublishedListsAreImmutable()
    {
        EmTrajectory trajectory = new EmTrajectoryAssembler().Assemble(
            CreatePath(TravelDirection.Forward, 0d, 0d), CreateLongitudinalResult(),
            CreateMetadata(TravelDirection.Forward, EmTerminalType.RollingSafetyStop));
        Verification.True(!(trajectory.Points is IList<EmTrajectoryPoint> mutable) || mutable.IsReadOnly,
            "trajectory public point list is immutable");
    }

    private static void VerifyKinematicFields(EmTrajectory trajectory, TravelDirection direction, string name)
    {
        double directionSign = direction == TravelDirection.Forward ? 1d : -1d;
        double previousTime = double.NegativeInfinity;
        double previousPathS = double.NegativeInfinity;
        for (int index = 0; index < trajectory.Points.Count; index++)
        {
            EmTrajectoryPoint point = trajectory.Points[index];
            Verification.NearlyEqual(Math.Abs(point.SignedLongitudinalVelocity), point.Speed,
                name + " speed field " + index);
            Verification.NearlyEqual(point.SignedLongitudinalVelocity * Math.Cos(point.Yaw), point.VelocityX,
                name + " velocity X field " + index);
            Verification.NearlyEqual(point.SignedLongitudinalVelocity * Math.Sin(point.Yaw), point.VelocityY,
                name + " velocity Y field " + index);
            Verification.NearlyEqual(point.SignedLongitudinalVelocity * point.VehicleCurvature, point.YawRate,
                name + " yaw-rate field " + index);
            if (index < 4)
                Verification.NearlyEqual(directionSign * CreateLongitudinalResult().Candidate.U[index],
                    point.SignedLongitudinalVelocity, name + " signed speed field " + index);
            Verification.True(point.TimeFromStart > previousTime, name + " time strictly increases " + index);
            Verification.True(point.PathS >= previousPathS, name + " PathS never decreases " + index);
            previousTime = point.TimeFromStart;
            previousPathS = point.PathS;
        }
    }

    private static void VerifyTerminalAndHold(EmTrajectory trajectory, EmBoundaryType terminalBoundary, string name)
    {
        const int terminalIndex = 3;
        EmTrajectoryPoint terminal = trajectory.Points[terminalIndex];
        Verification.Equal(terminalBoundary, terminal.BoundaryType, name + " exact terminal boundary type");
        Verification.NearlyEqual(0d, terminal.SignedLongitudinalVelocity, name + " exact terminal signed speed");
        Verification.NearlyEqual(0d, terminal.YawRate, name + " exact terminal yaw rate");
        Verification.NearlyEqual(0.137d, terminal.TimeFromStart, name + " exact non-regular terminal time");
        Verification.Equal(terminalIndex + 5, trajectory.Points.Count, name + " terminal plus 0.20-second hold samples");

        for (int index = terminalIndex + 1; index < trajectory.Points.Count; index++)
        {
            EmTrajectoryPoint hold = trajectory.Points[index];
            Verification.NearlyEqual(terminal.TimeFromStart + (index - terminalIndex) * 0.05d,
                hold.TimeFromStart, name + " hold timing " + index);
            Verification.NearlyEqual(terminal.X, hold.X, name + " hold X " + index);
            Verification.NearlyEqual(terminal.Y, hold.Y, name + " hold Y " + index);
            Verification.NearlyEqual(terminal.Yaw, hold.Yaw, name + " hold yaw " + index);
            Verification.NearlyEqual(0d, hold.SignedLongitudinalVelocity, name + " hold signed speed " + index);
            Verification.NearlyEqual(0d, hold.YawRate, name + " hold yaw rate " + index);
        }
    }

    private static LateralPath CreatePath(TravelDirection direction, double firstYaw, double lastYaw)
    {
        return new LateralPath(new[]
        {
            new LateralPathPoint(0d, 0d, 0d, 0d, 0d, 0d, 0d, 0d, firstYaw, 0d, 0.5d, 0d),
            new LateralPathPoint(1d, 0.12d, 0d, 0d, 0d, 0d, 0.12d, 0d, lastYaw, 0d, 0.5d, 0d),
        }, true);
    }

    private static LongitudinalPlanningResult CreateLongitudinalResult()
    {
        var candidate = new LongitudinalCandidate(
            new[] { 0d, 0.05d, 0.10d, 0.137d },
            new[] { 0d, 0.04d, 0.08d, 0.12d },
            new[] { 0.8d, 0.8d, 0.8d, 0d },
            new[] { 0d, 0d, 0d, 0d },
            new[] { 0d, 0d, 0d });
        return new LongitudinalPlanningResult(EmPlanningStatus.Success, candidate, string.Empty);
    }

    private static EmTrajectoryMetadata CreateMetadata(TravelDirection direction, EmTerminalType terminalType)
    {
        return new EmTrajectoryMetadata("trajectory", DateTimeOffset.UnixEpoch, DateTimeOffset.UnixEpoch, 3L,
            "reference", 4L, string.Empty, 2, direction, terminalType);
    }
}