From e12eb31205756f0e76904043ef30fb205eaf7b1e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Mon, 3 Aug 2026 22:14:37 +0800 Subject: [PATCH] feat: add EM planner contracts --- ClumsyPilot/ClumsyPilot.csproj | 6 ++ .../EMPlanner/Contracts/ContractNumeric.cs | 12 +++ .../EMPlanner/Contracts/EmBoundaryType.cs | 10 +++ .../EMPlanner/Contracts/EmMotionModel.cs | 8 ++ .../EMPlanner/Contracts/EmPlanningRequest.cs | 58 +++++++++++++ .../EMPlanner/Contracts/EmPlanningResult.cs | 28 +++++++ .../EMPlanner/Contracts/EmPlanningStatus.cs | 23 +++++ .../EMPlanner/Contracts/EmTerminalType.cs | 8 ++ .../EMPlanner/Contracts/EmTrajectory.cs | 33 ++++++++ .../Contracts/EmTrajectoryMetadata.cs | 57 +++++++++++++ .../EMPlanner/Contracts/EmTrajectoryPoint.cs | 84 +++++++++++++++++++ .../EMPlanner/Contracts/VehicleMotionState.cs | 42 ++++++++++ .../EMPlannerVerificationHost.csproj | 12 +++ .../FoundationChecks.cs | 51 +++++++++++ .../EMPlannerVerificationHost/Program.cs | 27 ++++++ .../EMPlannerVerificationHost/Verification.cs | 18 ++++ 16 files changed, 477 insertions(+) create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/ContractNumeric.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmBoundaryType.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmMotionModel.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningRequest.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningResult.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningStatus.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTerminalType.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectory.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectoryMetadata.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectoryPoint.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/VehicleMotionState.cs create mode 100644 ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj create mode 100644 ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs create mode 100644 ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs create mode 100644 ClumsyPilot/tests/EMPlannerVerificationHost/Verification.cs diff --git a/ClumsyPilot/ClumsyPilot.csproj b/ClumsyPilot/ClumsyPilot.csproj index 524db49..0592399 100644 --- a/ClumsyPilot/ClumsyPilot.csproj +++ b/ClumsyPilot/ClumsyPilot.csproj @@ -11,6 +11,12 @@ + + + + + ref\CommonUsage.dll diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/ContractNumeric.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/ContractNumeric.cs new file mode 100644 index 0000000..d8d0537 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/ContractNumeric.cs @@ -0,0 +1,12 @@ +using System; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +internal static class ContractNumeric +{ + public static void RequireFinite(double value, string parameterName) + { + if (double.IsNaN(value) || double.IsInfinity(value)) + throw new ArgumentOutOfRangeException(parameterName, "A finite value is required."); + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmBoundaryType.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmBoundaryType.cs new file mode 100644 index 0000000..275901a --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmBoundaryType.cs @@ -0,0 +1,10 @@ +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public enum EmBoundaryType +{ + None, + RollingSafetyStop, + GearSwitchApproach, + GearSwitchDeparture, + Goal, +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmMotionModel.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmMotionModel.cs new file mode 100644 index 0000000..64fd731 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmMotionModel.cs @@ -0,0 +1,8 @@ +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public enum EmMotionModel +{ + NonholonomicForwardReverse, + CrabTranslation, + InPlaceRotation, +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningRequest.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningRequest.cs new file mode 100644 index 0000000..a16b526 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningRequest.cs @@ -0,0 +1,58 @@ +using System; +using MultiWheelC.TrajectoryPlanning.CoarsePath; +using MultiWheelC.TrajectoryPlanning.Mapping; +using MultiWheelC.TrajectoryPlanning.PathSmoothing; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public sealed class EmPlanningRequest +{ + public EmPlanningRequest( + PathSmoothingResult referencePath, + PlanningGridMap map, + VehicleParameters vehicle, + VehicleMotionState vehicleState, + EmPlannerConfiguration configuration, + int segmentIndex, + EmTrajectory previousTrajectory, + DateTimeOffset requestedAtUtc, + DateTimeOffset effectiveAtUtc, + string outputTrajectoryId, + string referencePathId, + string previousTrajectoryId, + EmMotionModel motionModel) + { + ReferencePath = referencePath; + Map = map; + Vehicle = vehicle; + VehicleState = vehicleState; + Configuration = configuration; + SegmentIndex = segmentIndex; + PreviousTrajectory = previousTrajectory; + RequestedAtUtc = requestedAtUtc; + EffectiveAtUtc = effectiveAtUtc; + OutputTrajectoryId = outputTrajectoryId; + ReferencePathId = referencePathId; + PreviousTrajectoryId = previousTrajectoryId; + MotionModel = motionModel; + } + + public PathSmoothingResult ReferencePath { get; } + public PlanningGridMap Map { get; } + public VehicleParameters Vehicle { get; } + public VehicleMotionState VehicleState { get; } + public EmPlannerConfiguration Configuration { get; } + public int SegmentIndex { get; } + public EmTrajectory PreviousTrajectory { get; } + public DateTimeOffset RequestedAtUtc { get; } + public DateTimeOffset EffectiveAtUtc { get; } + public string OutputTrajectoryId { get; } + public string ReferencePathId { get; } + public string PreviousTrajectoryId { get; } + public EmMotionModel MotionModel { get; } +} + +// The request owns this DTO contract; Task 2 adds its request-bound settings. +public sealed partial class EmPlannerConfiguration +{ +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningResult.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningResult.cs new file mode 100644 index 0000000..e0e2f08 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningResult.cs @@ -0,0 +1,28 @@ +using System; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public sealed class EmPlanningResult +{ + public EmPlanningResult(EmPlanningStatus status, EmTrajectory trajectory, string failureReason) + { + if (!Enum.IsDefined(typeof(EmPlanningStatus), status)) + throw new ArgumentOutOfRangeException(nameof(status)); + + bool isSuccess = status == EmPlanningStatus.Success || status == EmPlanningStatus.SuccessWithFallback; + if (isSuccess && trajectory == null) + throw new ArgumentException("Successful results require a trajectory.", nameof(trajectory)); + if (!isSuccess && trajectory != null) + throw new ArgumentException("Only successful results may contain a trajectory.", nameof(trajectory)); + + Status = status; + Trajectory = trajectory; + FailureReason = failureReason ?? string.Empty; + } + + public EmPlanningStatus Status { get; } + + public EmTrajectory Trajectory { get; } + + public string FailureReason { get; } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningStatus.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningStatus.cs new file mode 100644 index 0000000..f0efedb --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmPlanningStatus.cs @@ -0,0 +1,23 @@ +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public enum EmPlanningStatus +{ + Success, + SuccessWithFallback, + InvalidInput, + UnsupportedMotionMode, + StaleVehicleState, + StateDirectionMismatch, + InvalidReferencePath, + ProjectionFailed, + CorridorInfeasible, + LateralInfeasible, + LongitudinalInfeasible, + StoppingDistanceInsufficient, + SolverUnavailable, + SolverTimedOut, + Cancelled, + ValidationFailed, + Superseded, + Failed, +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTerminalType.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTerminalType.cs new file mode 100644 index 0000000..089c74c --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTerminalType.cs @@ -0,0 +1,8 @@ +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public enum EmTerminalType +{ + RollingSafetyStop, + GearSwitch, + Goal, +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectory.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectory.cs new file mode 100644 index 0000000..c7da7d5 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectory.cs @@ -0,0 +1,33 @@ +using System; +using System.Collections.Generic; +using System.Collections.ObjectModel; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public sealed class EmTrajectory +{ + public EmTrajectory(EmTrajectoryMetadata metadata, IReadOnlyList points) + { + if (metadata == null) + throw new ArgumentNullException(nameof(metadata)); + if (points == null) + throw new ArgumentNullException(nameof(points)); + if (points.Count == 0) + throw new ArgumentException("A published trajectory requires at least one point.", nameof(points)); + + var copy = new List(points.Count); + for (int index = 0; index < points.Count; index++) + { + if (points[index] == null) + throw new ArgumentException("Trajectory points cannot contain null values.", nameof(points)); + copy.Add(points[index]); + } + + Metadata = metadata; + Points = new ReadOnlyCollection(copy); + } + + public EmTrajectoryMetadata Metadata { get; } + + public IReadOnlyList Points { get; } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectoryMetadata.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectoryMetadata.cs new file mode 100644 index 0000000..3433eb3 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectoryMetadata.cs @@ -0,0 +1,57 @@ +using System; +using MultiWheelC.TrajectoryPlanning.CoarsePath; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public sealed class EmTrajectoryMetadata +{ + public EmTrajectoryMetadata( + string trajectoryId, + DateTimeOffset generatedAtUtc, + DateTimeOffset effectiveAtUtc, + long mapSnapshotId, + string referencePathId, + long vehicleStateSequenceId, + string previousTrajectoryId, + int segmentIndex, + TravelDirection direction, + EmTerminalType terminalType) + { + if (string.IsNullOrWhiteSpace(trajectoryId)) + throw new ArgumentException("A trajectory ID is required.", nameof(trajectoryId)); + if (mapSnapshotId < 0) + throw new ArgumentOutOfRangeException(nameof(mapSnapshotId)); + if (string.IsNullOrWhiteSpace(referencePathId)) + throw new ArgumentException("A reference path ID is required.", nameof(referencePathId)); + if (vehicleStateSequenceId < 0) + throw new ArgumentOutOfRangeException(nameof(vehicleStateSequenceId)); + if (segmentIndex < 0) + throw new ArgumentOutOfRangeException(nameof(segmentIndex)); + if (!Enum.IsDefined(typeof(TravelDirection), direction)) + throw new ArgumentOutOfRangeException(nameof(direction)); + if (!Enum.IsDefined(typeof(EmTerminalType), terminalType)) + throw new ArgumentOutOfRangeException(nameof(terminalType)); + + TrajectoryId = trajectoryId; + GeneratedAtUtc = generatedAtUtc; + EffectiveAtUtc = effectiveAtUtc; + MapSnapshotId = mapSnapshotId; + ReferencePathId = referencePathId; + VehicleStateSequenceId = vehicleStateSequenceId; + PreviousTrajectoryId = previousTrajectoryId ?? string.Empty; + SegmentIndex = segmentIndex; + Direction = direction; + TerminalType = terminalType; + } + + public string TrajectoryId { get; } + public DateTimeOffset GeneratedAtUtc { get; } + public DateTimeOffset EffectiveAtUtc { get; } + public long MapSnapshotId { get; } + public string ReferencePathId { get; } + public long VehicleStateSequenceId { get; } + public string PreviousTrajectoryId { get; } + public int SegmentIndex { get; } + public TravelDirection Direction { get; } + public EmTerminalType TerminalType { get; } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectoryPoint.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectoryPoint.cs new file mode 100644 index 0000000..1cd136b --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectoryPoint.cs @@ -0,0 +1,84 @@ +using System; +using MultiWheelC.TrajectoryPlanning.CoarsePath; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public sealed class EmTrajectoryPoint +{ + public EmTrajectoryPoint( + double x, + double y, + double yaw, + double signedLongitudinalVelocity, + double timeFromStart, + double vehicleCurvature, + int segmentIndex, + double segmentLocalS, + double pathS, + TravelDirection direction, + EmBoundaryType boundaryType, + double longitudinalAcceleration, + double longitudinalJerk) + { + ContractNumeric.RequireFinite(x, nameof(x)); + ContractNumeric.RequireFinite(y, nameof(y)); + ContractNumeric.RequireFinite(yaw, nameof(yaw)); + ContractNumeric.RequireFinite(signedLongitudinalVelocity, nameof(signedLongitudinalVelocity)); + ContractNumeric.RequireFinite(timeFromStart, nameof(timeFromStart)); + ContractNumeric.RequireFinite(vehicleCurvature, nameof(vehicleCurvature)); + ContractNumeric.RequireFinite(segmentLocalS, nameof(segmentLocalS)); + ContractNumeric.RequireFinite(pathS, nameof(pathS)); + ContractNumeric.RequireFinite(longitudinalAcceleration, nameof(longitudinalAcceleration)); + ContractNumeric.RequireFinite(longitudinalJerk, nameof(longitudinalJerk)); + if (segmentIndex < 0) + throw new ArgumentOutOfRangeException(nameof(segmentIndex)); + if (timeFromStart < 0d) + throw new ArgumentOutOfRangeException(nameof(timeFromStart)); + if (segmentLocalS < 0d) + throw new ArgumentOutOfRangeException(nameof(segmentLocalS)); + if (pathS < 0d) + throw new ArgumentOutOfRangeException(nameof(pathS)); + if (!Enum.IsDefined(typeof(TravelDirection), direction)) + throw new ArgumentOutOfRangeException(nameof(direction)); + if (!Enum.IsDefined(typeof(EmBoundaryType), boundaryType)) + throw new ArgumentOutOfRangeException(nameof(boundaryType)); + + X = x; + Y = y; + Yaw = yaw; + SignedLongitudinalVelocity = signedLongitudinalVelocity; + Speed = Math.Abs(signedLongitudinalVelocity); + VelocityX = signedLongitudinalVelocity * Math.Cos(yaw); + VelocityY = signedLongitudinalVelocity * Math.Sin(yaw); + YawRate = signedLongitudinalVelocity * vehicleCurvature; + TimeFromStart = timeFromStart; + VehicleCurvature = vehicleCurvature; + SegmentIndex = segmentIndex; + SegmentLocalS = segmentLocalS; + PathS = pathS; + Direction = direction; + BoundaryType = boundaryType; + LongitudinalAcceleration = longitudinalAcceleration; + LongitudinalJerk = longitudinalJerk; + } + + public double X { get; } + public double Y { get; } + public double Yaw { get; } + public double SignedLongitudinalVelocity { get; } + public double Speed { get; } + public double VelocityX { get; } + public double VelocityY { get; } + public double YawRate { get; } + public double TimeFromStart { get; } + public double VehicleCurvature { get; } + public int SegmentIndex { get; } + public double SegmentLocalS { get; } + public double PathS { get; } + public TravelDirection Direction { get; } + public EmBoundaryType BoundaryType { get; } + + internal double LongitudinalAcceleration { get; } + + internal double LongitudinalJerk { get; } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/VehicleMotionState.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/VehicleMotionState.cs new file mode 100644 index 0000000..cb719f4 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/VehicleMotionState.cs @@ -0,0 +1,42 @@ +using System; +using MultiWheelC.TrajectoryPlanning.CoarsePath; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +public sealed class VehicleMotionState +{ + public VehicleMotionState( + Pose2D pose, + double signedLongitudinalSpeedMetersPerSecond, + double? longitudinalAccelerationMetersPerSecondSquared, + DateTimeOffset capturedAtUtc, + long sequenceId) + { + if (pose == null) + throw new ArgumentNullException(nameof(pose)); + ContractNumeric.RequireFinite(pose.X, nameof(pose)); + ContractNumeric.RequireFinite(pose.Y, nameof(pose)); + ContractNumeric.RequireFinite(pose.Heading, nameof(pose)); + ContractNumeric.RequireFinite(signedLongitudinalSpeedMetersPerSecond, nameof(signedLongitudinalSpeedMetersPerSecond)); + if (longitudinalAccelerationMetersPerSecondSquared.HasValue) + ContractNumeric.RequireFinite(longitudinalAccelerationMetersPerSecondSquared.Value, nameof(longitudinalAccelerationMetersPerSecondSquared)); + if (sequenceId < 0) + throw new ArgumentOutOfRangeException(nameof(sequenceId)); + + Pose = pose; + SignedLongitudinalSpeedMetersPerSecond = signedLongitudinalSpeedMetersPerSecond; + LongitudinalAccelerationMetersPerSecondSquared = longitudinalAccelerationMetersPerSecondSquared; + CapturedAtUtc = capturedAtUtc; + SequenceId = sequenceId; + } + + public Pose2D Pose { get; } + + public double SignedLongitudinalSpeedMetersPerSecond { get; } + + public double? LongitudinalAccelerationMetersPerSecondSquared { get; } + + public DateTimeOffset CapturedAtUtc { get; } + + public long SequenceId { get; } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj b/ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj new file mode 100644 index 0000000..c91072e --- /dev/null +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj @@ -0,0 +1,12 @@ + + + Exe + net10.0-windows + disable + enable + + + + + diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs new file mode 100644 index 0000000..433b15b --- /dev/null +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs @@ -0,0 +1,51 @@ +using System; +using MultiWheelC.TrajectoryPlanning.CoarsePath; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +internal static class FoundationChecks +{ + public static void Run() + { + var reverseState = new VehicleMotionState( + new Pose2D(1.5d, -2d, 0.25d), + -0.15d, + -0.03d, + new DateTimeOffset(2026, 8, 3, 0, 0, 0, TimeSpan.Zero), + 17L); + + EMPlannerVerificationHost.Verification.NearlyEqual(-0.15d, reverseState.SignedLongitudinalSpeedMetersPerSecond, + "reverse signed speed"); + + var point = new EmTrajectoryPoint( + 1.25d, + -0.75d, + 0.5d, + -0.12d, + 0.4d, + -0.2d, + 3, + 0.6d, + 0.8d, + TravelDirection.Reverse, + EmBoundaryType.GearSwitchApproach, + -0.04d, + 0.03d); + + EMPlannerVerificationHost.Verification.NearlyEqual(1.25d, point.X, "point x"); + EMPlannerVerificationHost.Verification.NearlyEqual(-0.75d, point.Y, "point y"); + EMPlannerVerificationHost.Verification.NearlyEqual(0.5d, point.Yaw, "point yaw"); + EMPlannerVerificationHost.Verification.NearlyEqual(-0.12d, point.SignedLongitudinalVelocity, "point signed speed"); + EMPlannerVerificationHost.Verification.NearlyEqual(0.4d, point.TimeFromStart, "point time"); + EMPlannerVerificationHost.Verification.NearlyEqual(-0.2d, point.VehicleCurvature, "point curvature"); + EMPlannerVerificationHost.Verification.Equal(3, point.SegmentIndex, "point segment index"); + EMPlannerVerificationHost.Verification.NearlyEqual(0.6d, point.SegmentLocalS, "point segment local s"); + EMPlannerVerificationHost.Verification.NearlyEqual(0.8d, point.PathS, "point path s"); + EMPlannerVerificationHost.Verification.Equal(TravelDirection.Reverse, point.Direction, "point direction"); + EMPlannerVerificationHost.Verification.Equal(EmBoundaryType.GearSwitchApproach, point.BoundaryType, "point boundary type"); + EMPlannerVerificationHost.Verification.NearlyEqual(0.12d, point.Speed, "point derived speed"); + EMPlannerVerificationHost.Verification.NearlyEqual(-0.12d * Math.Cos(0.5d), point.VelocityX, "point derived velocity x"); + EMPlannerVerificationHost.Verification.NearlyEqual(-0.12d * Math.Sin(0.5d), point.VelocityY, "point derived velocity y"); + EMPlannerVerificationHost.Verification.NearlyEqual((-0.12d) * (-0.2d), point.YawRate, "point derived yaw rate"); + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs new file mode 100644 index 0000000..6a165e9 --- /dev/null +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs @@ -0,0 +1,27 @@ +using System; + +namespace EMPlannerVerificationHost; + +internal static class Program +{ + private static int Main(string[] args) + { + if (args.Length != 1 || args[0] != "foundation") + { + Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation"); + return 2; + } + + try + { + MultiWheelC.TrajectoryPlanning.EMPlanner.FoundationChecks.Run(); + Console.WriteLine("PASS foundation"); + return 0; + } + catch (Exception exception) + { + Console.Error.WriteLine(exception); + return 1; + } + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/Verification.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/Verification.cs new file mode 100644 index 0000000..2acb888 --- /dev/null +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/Verification.cs @@ -0,0 +1,18 @@ +using System; + +namespace EMPlannerVerificationHost; + +internal static class Verification +{ + public static void Equal(T expected, T actual, string name) + { + if (!Equals(expected, actual)) + throw new InvalidOperationException(name + " expected " + expected + " but was " + actual + "."); + } + + public static void NearlyEqual(double expected, double actual, string name) + { + if (Math.Abs(expected - actual) > 1e-12d) + throw new InvalidOperationException(name + " expected " + expected + " but was " + actual + "."); + } +}