chore: save current workspace progress
This commit is contained in:
梁薄云
+13
@@ -0,0 +1,13 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
<PropertyGroup>
|
||||
<OutputType>Exe</OutputType>
|
||||
<TargetFramework>net10.0-windows</TargetFramework>
|
||||
<UseSystemDrawing>true</UseSystemDrawing>
|
||||
<ImplicitUsings>disable</ImplicitUsings>
|
||||
<Nullable>enable</Nullable>
|
||||
</PropertyGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\..\ClumsyPilot.csproj" />
|
||||
</ItemGroup>
|
||||
</Project>
|
||||
@@ -0,0 +1,360 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Globalization;
|
||||
using System.IO;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Test;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Visualization;
|
||||
|
||||
internal static class Program
|
||||
{
|
||||
private static readonly byte[] PngSignature = { 137, 80, 78, 71, 13, 10, 26, 10 };
|
||||
|
||||
private static int Main(string[] arguments)
|
||||
{
|
||||
try
|
||||
{
|
||||
if (arguments.Length == 3 && arguments[0] == "--export-fixtures")
|
||||
{
|
||||
ExportFixtureReports(arguments[1], arguments[2]);
|
||||
return 0;
|
||||
}
|
||||
if (arguments.Length == 2 && arguments[0] == "--export-end-to-end")
|
||||
{
|
||||
ExportEndToEndReports(arguments[1]);
|
||||
return 0;
|
||||
}
|
||||
Require(arguments.Length == 1 && File.Exists(arguments[0]), "A fixture path is required.");
|
||||
Verify(arguments[0]);
|
||||
Console.WriteLine("PNG verification host completed.");
|
||||
return 0;
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
Console.Error.WriteLine(exception.ToString());
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
private static void ExportFixtureReports(string fixturePath, string outputDirectory)
|
||||
{
|
||||
Require(File.Exists(fixturePath), "Fixture path was not found.");
|
||||
PrintReports(new PathSmoothingComparisonDemo().ExportFixtureReports(fixturePath, outputDirectory));
|
||||
}
|
||||
|
||||
private static void ExportEndToEndReports(string outputDirectory)
|
||||
{
|
||||
PrintReports(new PathSmoothingComparisonDemo().ExportEndToEndReports(outputDirectory));
|
||||
}
|
||||
|
||||
private static void PrintReports(IReadOnlyList<PathSmoothingComparisonScenarioResult> scenarios)
|
||||
{
|
||||
for (int scenarioIndex = 0; scenarioIndex < scenarios.Count; scenarioIndex++)
|
||||
{
|
||||
PathSmoothingComparisonScenarioResult scenario = scenarios[scenarioIndex];
|
||||
if (scenario.Comparison == null)
|
||||
{
|
||||
Console.WriteLine(scenario.ScenarioId + " coarse=" + scenario.CoarsePathStatus + " " + scenario.Diagnostic);
|
||||
continue;
|
||||
}
|
||||
PrintEntry(scenario.ScenarioId, scenario.Comparison.RawPathBaseline);
|
||||
for (int entryIndex = 0; entryIndex < scenario.Comparison.Entries.Count; entryIndex++)
|
||||
PrintEntry(scenario.ScenarioId, scenario.Comparison.Entries[entryIndex]);
|
||||
Console.WriteLine(scenario.ScenarioId + " report=" + scenario.Report.Status + " " + scenario.Report.Reason);
|
||||
}
|
||||
}
|
||||
|
||||
private static void PrintEntry(string scenarioId, PathSmoothingComparisonEntry entry)
|
||||
{
|
||||
Console.WriteLine(string.Format(
|
||||
CultureInfo.InvariantCulture,
|
||||
"{0} {1} status={2} length={3:F4} peakKappa={4:F4} clearance={5:F4}",
|
||||
scenarioId,
|
||||
entry.IsRawPathBaseline ? "RawPath" : entry.Method.ToString(),
|
||||
entry.Status,
|
||||
entry.Metrics.PathLengthMeters,
|
||||
entry.Metrics.MaximumAbsoluteVehicleCurvaturePerMeter,
|
||||
entry.Metrics.MinimumBodyClearanceMeters));
|
||||
}
|
||||
|
||||
private static void Verify(string fixturePath)
|
||||
{
|
||||
PathSmoothingComparisonRequest request = CreateComparisonRequest();
|
||||
PathSmoothingComparisonResult comparison = new PathSmoothingComparisonService().Compare(request);
|
||||
Require(!comparison.IsCancelled, "Fixture comparison was cancelled.");
|
||||
Require(request.SmoothingRequest.CoarsePath.Count > 1, "Fixture coarse path is too short.");
|
||||
|
||||
CoarsePathPoint first = request.SmoothingRequest.CoarsePath[0];
|
||||
CoarsePathPoint last = request.SmoothingRequest.CoarsePath[request.SmoothingRequest.CoarsePath.Count - 1];
|
||||
var model = new SmoothingFigureModelBuilder().Build(
|
||||
comparison,
|
||||
request.SmoothingRequest.Map,
|
||||
new Pose2D(first.X, first.Y, first.Heading),
|
||||
new Pose2D(last.X, last.Y, last.Heading),
|
||||
"straight",
|
||||
"Straight");
|
||||
|
||||
VerifyObstacleRectanglesAreMerged(fixturePath, comparison, new Pose2D(first.X, first.Y, first.Heading), new Pose2D(last.X, last.Y, last.Heading));
|
||||
VerifySixFigureDefinitionContract(model);
|
||||
VerifyPointOnlyRendererContract(model);
|
||||
|
||||
var resolver = new SmoothingFontResolver();
|
||||
Require(resolver.TryResolve("SimSun", "Times New Roman", out SmoothingFontResolution fonts, out string fontReason),
|
||||
"Required report fonts are unavailable: " + fontReason);
|
||||
byte[] png;
|
||||
using (fonts)
|
||||
{
|
||||
Require(fonts.ChineseFamilyName == "SimSun", "Chinese font must resolve to exact SimSun family.");
|
||||
Require(fonts.LatinFamilyName == "Times New Roman", "Latin font must resolve to exact Times New Roman family.");
|
||||
Require(resolver.MeasureMixedText(fonts, "粗路径 κ(s) X (m) −π", 9f).Width > 0f,
|
||||
"Mixed Chinese/Latin sample must have nonempty measured bounds.");
|
||||
png = new SmoothingPngRenderer().Render(model, fonts);
|
||||
}
|
||||
VerifyPng(png);
|
||||
|
||||
string outputDirectory = Path.Combine(Path.GetTempPath(), "path-smoothing-png-" + Guid.NewGuid().ToString("N"));
|
||||
try
|
||||
{
|
||||
var exporter = new SmoothingReportExporter();
|
||||
SmoothingReportExportResult report = exporter.Export(new SmoothingReportExportRequest
|
||||
{
|
||||
Model = model,
|
||||
OutputDirectory = outputDirectory,
|
||||
FileStem = "straight-report",
|
||||
});
|
||||
Require(report.Status == SmoothingReportExportStatus.Success, "Report export failed: " + report.Reason);
|
||||
Require(report.SvgPaths.Count == 6 && report.PngPaths.Count == 6 && File.Exists(report.CsvPath),
|
||||
"Successful export must publish six SVGs, six PNGs and one CSV.");
|
||||
VerifyPublishedSixFigureFiles(report, outputDirectory);
|
||||
Require(!ContainsTemporaryFiles(outputDirectory), "Successful export must not leave temporary files.");
|
||||
|
||||
string unavailableDirectory = Path.Combine(outputDirectory, "missing-font");
|
||||
SmoothingReportExportResult unavailable = exporter.Export(new SmoothingReportExportRequest
|
||||
{
|
||||
Model = model,
|
||||
OutputDirectory = unavailableDirectory,
|
||||
FileStem = "must-not-write",
|
||||
ChineseFontFamilyName = "Missing report font",
|
||||
LatinFontFamilyName = "Times New Roman",
|
||||
});
|
||||
Require(unavailable.Status == SmoothingReportExportStatus.FontUnavailable,
|
||||
"Missing exact font must report FontUnavailable.");
|
||||
Require(!Directory.Exists(unavailableDirectory), "Missing-font export must remain atomic and create no output directory.");
|
||||
}
|
||||
finally
|
||||
{
|
||||
if (Directory.Exists(outputDirectory)) Directory.Delete(outputDirectory, true);
|
||||
}
|
||||
}
|
||||
|
||||
private static PathSmoothingComparisonRequest CreateComparisonRequest()
|
||||
{
|
||||
PlanningMapBuildResult mapResult = new PlanningMapFactory().Create(new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(0f, 5000f, 0f, 5000f),
|
||||
ResolutionMm = 50f,
|
||||
AllowExplicitEmptyMap = true,
|
||||
});
|
||||
Require(mapResult.Succeeded && mapResult.Map != null, "PNG verification map must be created.");
|
||||
var vehicle = new VehicleParameters
|
||||
{
|
||||
LengthMeters = 0.20d,
|
||||
WidthMeters = 0.20d,
|
||||
SafetyMarginMeters = 0d,
|
||||
MaximumCurvaturePerMeter = 100d,
|
||||
};
|
||||
var coarsePath = new List<CoarsePathPoint>
|
||||
{
|
||||
CreatePoint(0.5d, 0.5d, 0d),
|
||||
CreatePoint(1.0d, 0.5d, 0.5d),
|
||||
CreatePoint(1.0d, 1.0d, 1.0d),
|
||||
CreatePoint(1.5d, 1.0d, 1.5d),
|
||||
};
|
||||
var segments = new List<PathSegment>
|
||||
{
|
||||
new PathSegment(0, TravelDirection.Forward, 0, coarsePath.Count - 1, false, false),
|
||||
};
|
||||
return new PathSmoothingComparisonRequest(new PathSmoothingRequest(
|
||||
coarsePath,
|
||||
segments,
|
||||
mapResult.Map,
|
||||
vehicle,
|
||||
new PathSmoothingConfiguration()));
|
||||
}
|
||||
|
||||
private static void VerifySixFigureDefinitionContract(SmoothingFigureModel model)
|
||||
{
|
||||
SmoothingFigureSet figureSet = new SmoothingFigureSetBuilder().Build(model);
|
||||
var stems = new List<string>();
|
||||
for (int index = 0; index < figureSet.Figures.Count; index++) stems.Add(figureSet.Figures[index].FileStem);
|
||||
string expected = string.Join(",", new[]
|
||||
{
|
||||
"01-coarse-path-overview",
|
||||
"02-all-paths-comparison",
|
||||
"03-cubic-bspline-overview",
|
||||
"04-local-cubic-bezier-overview",
|
||||
"05-piecewise-quintic-overview",
|
||||
"06-curvature-comparison",
|
||||
});
|
||||
Require(string.Join(",", stems) == expected, "Six-figure report stems must be stable and ordered.");
|
||||
Require(!figureSet.Figures[1].ShowsMapContext, "All-path comparison must contain only trajectories, axes and legend.");
|
||||
Require(figureSet.Figures[2].ShowsMapContext && figureSet.Figures[2].Series[0].Opacity < 1d,
|
||||
"Individual smoother figures must retain a faded raw-path map reference.");
|
||||
Require(figureSet.Figures[0].WorldScalePointsPerMeter > 0d &&
|
||||
Math.Abs((figureSet.Figures[0].WorldXMaxMeters - figureSet.Figures[0].WorldXMinMeters) /
|
||||
(figureSet.Figures[0].WorldYMaxMeters - figureSet.Figures[0].WorldYMinMeters) -
|
||||
figureSet.Figures[0].PlotWidthPoints / figureSet.Figures[0].PlotHeightPoints) < 0.000001d,
|
||||
"Overhead figures must preserve equal X/Y scale.");
|
||||
Require(figureSet.Figures[0].LegendYPoints -
|
||||
(figureSet.Figures[0].PlotYPoints + figureSet.Figures[0].PlotHeightPoints + 31d) >= 8d,
|
||||
"Legend must leave vertical clearance below the X-axis unit label.");
|
||||
}
|
||||
|
||||
private static void VerifyObstacleRectanglesAreMerged(string fixturePath, PathSmoothingComparisonResult comparison, Pose2D start, Pose2D goal)
|
||||
{
|
||||
IReadOnlyList<SmoothingScenarioFixture> fixtures = SmoothingScenarioFixtureLoader.LoadAndVerify(fixturePath);
|
||||
IReadOnlyList<PathSmoothingComparisonRequest> requests = SmoothingScenarioFactory.CreateFixtureRequests(fixturePath);
|
||||
int rectangleIndex = -1;
|
||||
for (int index = 0; index < fixtures.Count; index++)
|
||||
{
|
||||
if (fixtures[index].Id == "rectangle-detour") { rectangleIndex = index; break; }
|
||||
}
|
||||
Require(rectangleIndex >= 0, "Fixture suite must include rectangle-detour for obstacle rendering verification.");
|
||||
SmoothingFigureModel model = new SmoothingFigureModelBuilder().Build(
|
||||
comparison, requests[rectangleIndex].SmoothingRequest.Map, start, goal, "obstacle", "obstacle");
|
||||
Require(model.Obstacles.Count > 0, "Rectangle-detour fixture must produce report obstacles.");
|
||||
for (int firstIndex = 0; firstIndex < model.Obstacles.Count; firstIndex++)
|
||||
{
|
||||
SmoothingFigureObstacle first = model.Obstacles[firstIndex];
|
||||
for (int secondIndex = firstIndex + 1; secondIndex < model.Obstacles.Count; secondIndex++)
|
||||
{
|
||||
SmoothingFigureObstacle second = model.Obstacles[secondIndex];
|
||||
bool matchingColumn = Math.Abs(first.X - second.X) < 0.0000001d && Math.Abs(first.Width - second.Width) < 0.0000001d;
|
||||
bool verticallyAdjacent = Math.Abs((first.Y + first.Height) - second.Y) < 0.0000001d ||
|
||||
Math.Abs((second.Y + second.Height) - first.Y) < 0.0000001d;
|
||||
Require(!(matchingColumn && verticallyAdjacent), "Adjacent occupied rows must merge into a single obstacle rectangle.");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private static void VerifyPointOnlyRendererContract(SmoothingFigureModel model)
|
||||
{
|
||||
SmoothingFigureDefinition figure = new SmoothingFigureSetBuilder().Build(model).Figures[1];
|
||||
string svg = new SmoothingSvgRenderer().Render(figure);
|
||||
Require(svg.Contains("class=\"trajectory-point\""), "Trajectory samples must render as discrete SVG point markers.");
|
||||
Require(!svg.Contains("stroke-dasharray") && !svg.Contains("-path\""), "Trajectory SVG output must not use dashed or joined path strokes.");
|
||||
Require(svg.Contains("X (m)") && svg.Contains("Y (m)"), "Overhead SVG must label metre coordinate axes.");
|
||||
}
|
||||
|
||||
private static void VerifyPublishedSixFigureFiles(SmoothingReportExportResult report, string outputDirectory)
|
||||
{
|
||||
var expectedStems = new[]
|
||||
{
|
||||
"01-coarse-path-overview",
|
||||
"02-all-paths-comparison",
|
||||
"03-cubic-bspline-overview",
|
||||
"04-local-cubic-bezier-overview",
|
||||
"05-piecewise-quintic-overview",
|
||||
"06-curvature-comparison",
|
||||
};
|
||||
VerifyPublishedPaths(report.SvgPaths, expectedStems, ".svg", outputDirectory);
|
||||
VerifyPublishedPaths(report.PngPaths, expectedStems, ".png", outputDirectory);
|
||||
Require(!File.Exists(Path.Combine(outputDirectory, "comparison.svg")) && !File.Exists(Path.Combine(outputDirectory, "comparison.png")),
|
||||
"Six-figure export must not leave legacy composite comparison images.");
|
||||
}
|
||||
|
||||
private static void VerifyPublishedPaths(IReadOnlyList<string> paths, string[] expectedStems, string extension, string outputDirectory)
|
||||
{
|
||||
var actual = new List<string>();
|
||||
for (int index = 0; index < paths.Count; index++) actual.Add(paths[index]);
|
||||
Require(actual.Count == expectedStems.Length, "Six-figure export must publish six " + extension + " files.");
|
||||
for (int index = 0; index < expectedStems.Length; index++)
|
||||
{
|
||||
string expected = Path.Combine(outputDirectory, expectedStems[index] + extension);
|
||||
Require(actual[index] == expected && File.Exists(actual[index]), "Published " + extension + " path must match the stable figure stem.");
|
||||
}
|
||||
}
|
||||
|
||||
private static CoarsePathPoint CreatePoint(double x, double y, double arcLength)
|
||||
{
|
||||
return new CoarsePathPoint(
|
||||
x, y, 0d, 0d, arcLength, TravelDirection.Forward, 0d, 1d, false, CoarsePathPointSource.Start);
|
||||
}
|
||||
|
||||
private static bool ContainsTemporaryFiles(string directory)
|
||||
{
|
||||
foreach (string ignored in Directory.EnumerateFiles(directory, "*.tmp")) return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
private static void VerifyPng(byte[] png)
|
||||
{
|
||||
Require(png != null && png.Length > PngSignature.Length, "PNG output is empty.");
|
||||
byte[] data = png ?? throw new InvalidOperationException("PNG output is empty.");
|
||||
for (int index = 0; index < PngSignature.Length; index++)
|
||||
Require(data[index] == PngSignature[index], "PNG signature is invalid.");
|
||||
|
||||
bool sawHeader = false;
|
||||
bool sawPhysicalResolution = false;
|
||||
int offset = PngSignature.Length;
|
||||
while (offset < data.Length)
|
||||
{
|
||||
Require(offset + 12 <= data.Length, "PNG chunk header is truncated.");
|
||||
int length = checked((int)ReadUInt32BigEndian(data, offset));
|
||||
int dataStart = offset + 8;
|
||||
int crcStart = checked(dataStart + length);
|
||||
Require(crcStart + 4 <= data.Length, "PNG chunk data is truncated.");
|
||||
uint expectedCrc = ReadUInt32BigEndian(data, crcStart);
|
||||
uint actualCrc = ComputeCrc32(data, offset + 4, length + 4);
|
||||
Require(expectedCrc == actualCrc, "PNG chunk CRC is invalid.");
|
||||
|
||||
string type = System.Text.Encoding.ASCII.GetString(data, offset + 4, 4);
|
||||
if (type == "IHDR")
|
||||
{
|
||||
Require(length == 13, "IHDR length must be 13.");
|
||||
Require(ReadUInt32BigEndian(data, dataStart) == SmoothingPngRenderer.WidthPixels, "PNG width must be 4296.");
|
||||
Require(ReadUInt32BigEndian(data, dataStart + 4) == SmoothingPngRenderer.HeightPixels, "PNG height must be 3120.");
|
||||
sawHeader = true;
|
||||
}
|
||||
else if (type == "pHYs")
|
||||
{
|
||||
Require(length == 9, "pHYs length must be 9.");
|
||||
Require(ReadUInt32BigEndian(data, dataStart) == SmoothingPngRenderer.PixelsPerMeter,
|
||||
"PNG horizontal density must be 23622 pixels/meter.");
|
||||
Require(ReadUInt32BigEndian(data, dataStart + 4) == SmoothingPngRenderer.PixelsPerMeter,
|
||||
"PNG vertical density must be 23622 pixels/meter.");
|
||||
Require(data[dataStart + 8] == 1, "PNG pHYs unit must be meter.");
|
||||
sawPhysicalResolution = true;
|
||||
}
|
||||
offset = crcStart + 4;
|
||||
}
|
||||
Require(sawHeader && sawPhysicalResolution, "PNG must contain IHDR and pHYs chunks.");
|
||||
}
|
||||
|
||||
private static uint ReadUInt32BigEndian(byte[] data, int offset)
|
||||
{
|
||||
return ((uint)data[offset] << 24) | ((uint)data[offset + 1] << 16) |
|
||||
((uint)data[offset + 2] << 8) | data[offset + 3];
|
||||
}
|
||||
|
||||
private static uint ComputeCrc32(byte[] data, int offset, int length)
|
||||
{
|
||||
uint crc = 0xffffffffu;
|
||||
for (int index = 0; index < length; index++)
|
||||
{
|
||||
crc ^= data[offset + index];
|
||||
for (int bit = 0; bit < 8; bit++)
|
||||
crc = (crc & 1u) == 0u ? crc >> 1 : (crc >> 1) ^ 0xedb88320u;
|
||||
}
|
||||
return crc ^ 0xffffffffu;
|
||||
}
|
||||
|
||||
private static void Require(bool condition, string message)
|
||||
{
|
||||
if (!condition) throw new InvalidOperationException(message);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,15 @@
|
||||
param(
|
||||
[string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'),
|
||||
[string]$OutputPath = (Join-Path $PSScriptRoot '..\ParkrobTrajplanner\PathSmoothing\Test\Fixtures\path-smoothing-fixtures.json'),
|
||||
[switch]$Overwrite
|
||||
)
|
||||
|
||||
$ErrorActionPreference = 'Stop'
|
||||
$newtonsoftPath = Join-Path $env:USERPROFILE '.nuget\packages\newtonsoft.json\13.0.4\lib\netstandard2.0\Newtonsoft.Json.dll'
|
||||
if (Test-Path -LiteralPath $newtonsoftPath) { [Reflection.Assembly]::LoadFrom($newtonsoftPath) | Out-Null }
|
||||
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
|
||||
$type = $assembly.GetType('MultiWheelC.TrajectoryPlanning.PathSmoothing.Test.SmoothingFixtureGenerator', $true)
|
||||
$method = $type.GetMethod('Generate', [Type[]]@([string], [bool]))
|
||||
if ($null -eq $method) { throw 'SmoothingFixtureGenerator.Generate(string, bool) is missing.' }
|
||||
$method.Invoke($null, @([IO.Path]::GetFullPath($OutputPath), [bool]$Overwrite))
|
||||
Write-Output "Path smoothing fixtures generated: $OutputPath"
|
||||
@@ -0,0 +1,32 @@
|
||||
param(
|
||||
[switch]$FixtureOnly,
|
||||
[string]$FixturePath = (Join-Path $PSScriptRoot '..\ParkrobTrajplanner\PathSmoothing\Test\Fixtures\path-smoothing-fixtures.json'),
|
||||
[string]$OutputDirectory = (Join-Path $PSScriptRoot '..\obj\path_smoothing_reports')
|
||||
)
|
||||
|
||||
$ErrorActionPreference = 'Stop'
|
||||
|
||||
$clumsyPilotRoot = (Resolve-Path -LiteralPath (Join-Path $PSScriptRoot '..')).Path
|
||||
$allowedRoot = [IO.Path]::GetFullPath((Join-Path $clumsyPilotRoot 'obj\path_smoothing_reports'))
|
||||
$resolvedOutputDirectory = [IO.Path]::GetFullPath($OutputDirectory)
|
||||
$allowedPrefix = $allowedRoot.TrimEnd([IO.Path]::DirectorySeparatorChar, [IO.Path]::AltDirectorySeparatorChar) + [IO.Path]::DirectorySeparatorChar
|
||||
if ($resolvedOutputDirectory -ne $allowedRoot -and -not $resolvedOutputDirectory.StartsWith($allowedPrefix, [StringComparison]::OrdinalIgnoreCase)) {
|
||||
throw "Report output must stay below $allowedRoot"
|
||||
}
|
||||
|
||||
$projectPath = Join-Path $clumsyPilotRoot 'ClumsyPilot.csproj'
|
||||
$hostProject = Join-Path $PSScriptRoot 'PathSmoothingPngVerificationHost\PathSmoothingPngVerificationHost.csproj'
|
||||
$resolvedFixturePath = (Resolve-Path -LiteralPath $FixturePath).Path
|
||||
|
||||
& dotnet build $projectPath --no-restore
|
||||
if ($LASTEXITCODE -ne 0) { exit $LASTEXITCODE }
|
||||
|
||||
& dotnet run --project $hostProject --no-restore -- --export-fixtures $resolvedFixturePath $resolvedOutputDirectory
|
||||
if ($LASTEXITCODE -ne 0) { exit $LASTEXITCODE }
|
||||
|
||||
if (-not $FixtureOnly) {
|
||||
& dotnet run --project $hostProject --no-restore -- --export-end-to-end $resolvedOutputDirectory
|
||||
if ($LASTEXITCODE -ne 0) { exit $LASTEXITCODE }
|
||||
}
|
||||
|
||||
Write-Output "Path smoothing reports written below $resolvedOutputDirectory"
|
||||
@@ -0,0 +1,130 @@
|
||||
param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
|
||||
$ErrorActionPreference = 'Stop'
|
||||
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
|
||||
|
||||
function Assert-True($Actual, [string]$Message) {
|
||||
if (-not $Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-False($Actual, [string]$Message) {
|
||||
if ($Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-Near([double]$Expected, [double]$Actual, [string]$Message) {
|
||||
if ([Math]::Abs($Expected - $Actual) -gt 0.000001) { throw "$Message Expected=$Expected Actual=$Actual" }
|
||||
}
|
||||
|
||||
function New-TestMap {
|
||||
$mapping = 'MultiWheelC.TrajectoryPlanning.Mapping.'
|
||||
$boundsType = $assembly.GetType($mapping + 'MapBoundsMm', $true)
|
||||
$obstacleType = $assembly.GetType($mapping + 'IMapObstacle', $true)
|
||||
$rectangleType = $assembly.GetType($mapping + 'AxisAlignedRectangleObstacle', $true)
|
||||
$sourceType = $assembly.GetType($mapping + 'IMapObstacleSource', $true)
|
||||
$manualType = $assembly.GetType($mapping + 'ManualObstacleSource', $true)
|
||||
$requestType = $assembly.GetType($mapping + 'PlanningMapRequest', $true)
|
||||
$factoryType = $assembly.GetType($mapping + 'PlanningMapFactory', $true)
|
||||
$bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]3000, [single]0, [single]3000))
|
||||
$obstacle = [Activator]::CreateInstance($rectangleType, @([single]1500, [single]1550, [single]1200, [single]1800))
|
||||
$obstacles = [Array]::CreateInstance($obstacleType, 1)
|
||||
$obstacles.SetValue($obstacle, 0)
|
||||
$source = [Activator]::CreateInstance($manualType, @('manual', [long]1, $true, $obstacles))
|
||||
$sources = [Array]::CreateInstance($sourceType, 1)
|
||||
$sources.SetValue($source, 0)
|
||||
$request = [Activator]::CreateInstance($requestType)
|
||||
$request.Bounds = $bounds
|
||||
$request.ResolutionMm = [single]50
|
||||
$request.ObstacleSources = $sources
|
||||
$request.AllowExplicitEmptyMap = $false
|
||||
$result = [Activator]::CreateInstance($factoryType).Create($request)
|
||||
Assert-True $result.Succeeded 'Test map must be created.'
|
||||
Assert-True $result.Map.PlanningReady 'Test map must be ready.'
|
||||
return $result.Map
|
||||
}
|
||||
|
||||
function New-DiagonalCellMap {
|
||||
$mapping = 'MultiWheelC.TrajectoryPlanning.Mapping.'
|
||||
$boundsType = $assembly.GetType($mapping + 'MapBoundsMm', $true)
|
||||
$obstacleType = $assembly.GetType($mapping + 'IMapObstacle', $true)
|
||||
$circleType = $assembly.GetType($mapping + 'CircleObstacle', $true)
|
||||
$sourceType = $assembly.GetType($mapping + 'IMapObstacleSource', $true)
|
||||
$manualType = $assembly.GetType($mapping + 'ManualObstacleSource', $true)
|
||||
$requestType = $assembly.GetType($mapping + 'PlanningMapRequest', $true)
|
||||
$factoryType = $assembly.GetType($mapping + 'PlanningMapFactory', $true)
|
||||
$bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]3000, [single]0, [single]3000))
|
||||
$obstacle = [Activator]::CreateInstance($circleType, @([single]1525, [single]1525, [single]0))
|
||||
$obstacles = [Array]::CreateInstance($obstacleType, 1)
|
||||
$obstacles.SetValue($obstacle, 0)
|
||||
$source = [Activator]::CreateInstance($manualType, @('diagonal-cell', [long]1, $true, $obstacles))
|
||||
$sources = [Array]::CreateInstance($sourceType, 1)
|
||||
$sources.SetValue($source, 0)
|
||||
$request = [Activator]::CreateInstance($requestType)
|
||||
$request.Bounds = $bounds
|
||||
$request.ResolutionMm = [single]50
|
||||
$request.ObstacleSources = $sources
|
||||
$request.AllowExplicitEmptyMap = $false
|
||||
$result = [Activator]::CreateInstance($factoryType).Create($request)
|
||||
Assert-True $result.Succeeded 'Diagonal cell map must be created.'
|
||||
Assert-True $result.Map.PlanningReady 'Diagonal cell map must be ready.'
|
||||
Assert-True $result.Map.IsOccupied(30, 30) 'Diagonal cell obstacle must occupy its 50mm cell.'
|
||||
return $result.Map
|
||||
}
|
||||
|
||||
$map = New-TestMap
|
||||
$vehicle = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.VehicleParameters
|
||||
$vehicle.LengthMeters = 0.20
|
||||
$vehicle.WidthMeters = 0.20
|
||||
$vehicle.SafetyMarginMeters = 0.0
|
||||
$checker = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle.FootprintCollisionChecker
|
||||
|
||||
$diagonalCellMap = New-DiagonalCellMap
|
||||
$diagonalCellVehicle = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.VehicleParameters
|
||||
$diagonalCellVehicle.LengthMeters = 0.10
|
||||
$diagonalCellVehicle.WidthMeters = 0.10
|
||||
$diagonalCellVehicle.SafetyMarginMeters = 0.0
|
||||
$diagonalCellPose = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Pose2D(1.299, 1.299, 0.0)
|
||||
$diagonalCellClearance = 0.0
|
||||
$diagonalCellSafe = $checker.IsPoseCollisionFree($diagonalCellPose, $diagonalCellMap, $diagonalCellVehicle, 0.20, [ref]$diagonalCellClearance)
|
||||
Assert-False $diagonalCellSafe 'Expanded diagonal footprint must not bypass an occupied 50mm cell.'
|
||||
|
||||
$edgePose = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Pose2D(1.35, 1.50, 0.0)
|
||||
$edgeClearance = 0.0
|
||||
$edgeSafe = $checker.IsPoseCollisionFree($edgePose, $map, $vehicle, 0.0, [ref]$edgeClearance)
|
||||
Assert-False $edgeSafe 'Touching an occupied cell must be a collision.'
|
||||
|
||||
$farPose = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Pose2D(0.50, 0.50, 0.0)
|
||||
$farClearance = 0.0
|
||||
$farSafe = $checker.IsPoseCollisionFree($farPose, $map, $vehicle, 0.0, [ref]$farClearance)
|
||||
Assert-True $farSafe 'Strict distance field clearance must allow the distant pose.'
|
||||
Assert-True ($farClearance -gt 0.0) 'Distant pose must report positive body clearance.'
|
||||
|
||||
$from = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Pose2D(1.20, 1.50, 0.0)
|
||||
$to = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Pose2D(1.80, 1.50, 0.0)
|
||||
$sweepClearance = 0.0
|
||||
$sweepSafe = $checker.IsSweptMotionCollisionFree($from, $to, $map, $vehicle, 0.50, [ref]$sweepClearance)
|
||||
Assert-False $sweepSafe 'A swept vehicle must not pass through an occupied cell.'
|
||||
|
||||
$gridCenterPose = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Pose2D(1.475, 1.475, 0.0)
|
||||
$gridCenterClearance = 0.0
|
||||
Assert-False $checker.IsPoseCollisionFree($gridCenterPose, $map, $vehicle, 0.0, [ref]$gridCenterClearance) 'A pose at an occupied grid center must collide.'
|
||||
|
||||
$subGridPose = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Pose2D(1.481, 1.463, 0.37)
|
||||
$subGridClearance = 0.0
|
||||
Assert-False $checker.IsPoseCollisionFree($subGridPose, $map, $vehicle, 0.0, [ref]$subGridClearance) 'An arbitrary sub-grid heading must collide with the thin obstacle.'
|
||||
|
||||
$diagonalPose = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Pose2D(1.32, 1.50, ([Math]::PI / 4.0))
|
||||
$diagonalClearance = 0.0
|
||||
Assert-False $checker.IsPoseCollisionFree($diagonalPose, $map, $vehicle, 0.0, [ref]$diagonalClearance) 'A 45 degree footprint overlap must collide.'
|
||||
|
||||
$outsidePose = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.Pose2D(0.05, 0.05, 0.0)
|
||||
$outsideClearance = 0.0
|
||||
Assert-False $checker.IsPoseCollisionFree($outsidePose, $map, $vehicle, 0.0, [ref]$outsideClearance) 'Any footprint corner outside the map must collide.'
|
||||
|
||||
$curveVehicle = New-Object MultiWheelC.TrajectoryPlanning.CoarsePath.VehicleParameters
|
||||
$curveVehicle.MaximumCurvaturePerMeter = 0.80
|
||||
$curveVehicle.MinimumTurningRadiusMeters = 2.0
|
||||
$maximumCurvature = 0.0
|
||||
$hasMaximumCurvature = [MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle.VehicleKinematics]::TryGetMaximumCurvaturePerMeter($curveVehicle, [ref]$maximumCurvature)
|
||||
Assert-True $hasMaximumCurvature 'Vehicle curvature constraints must be accepted.'
|
||||
Assert-Near 0.50 $maximumCurvature 'Both curvature constraints must use the conservative minimum.'
|
||||
|
||||
Write-Output 'Coarse path collision checks passed.'
|
||||
@@ -0,0 +1,460 @@
|
||||
param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
|
||||
$ErrorActionPreference = 'Stop'
|
||||
|
||||
$plannerRoot = Join-Path $PSScriptRoot '..\ParkrobTrajplanner'
|
||||
$coarsePathReadme = Join-Path $plannerRoot 'CoarsePath\README.md'
|
||||
$legacyRootReadme = Join-Path $plannerRoot 'README.md'
|
||||
if (-not (Test-Path -LiteralPath $coarsePathReadme -PathType Leaf)) {
|
||||
throw 'CoarsePath module README must be located inside the CoarsePath directory.'
|
||||
}
|
||||
if (Test-Path -LiteralPath $legacyRootReadme -PathType Leaf) {
|
||||
throw 'The coarse-path-only README must not remain at the ParkrobTrajplanner root.'
|
||||
}
|
||||
$coarsePathReadmeContent = Get-Content -LiteralPath $coarsePathReadme -Raw
|
||||
if (-not $coarsePathReadmeContent.Contains('## 总预算与取消')) {
|
||||
throw 'CoarsePath README must document total timeout and cancellation semantics.'
|
||||
}
|
||||
if (-not $coarsePathReadmeContent.Contains('PlanningMapBuildStatus')) {
|
||||
throw 'CoarsePath README must document map-stage termination status handling.'
|
||||
}
|
||||
|
||||
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
|
||||
|
||||
function Assert-True($Actual, [string]$Message) {
|
||||
if (-not $Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-False($Actual, [string]$Message) {
|
||||
if ($Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-Equal($Expected, $Actual, [string]$Message) {
|
||||
if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" }
|
||||
}
|
||||
|
||||
function Assert-Near([double]$Expected, [double]$Actual, [string]$Message) {
|
||||
if ([Math]::Abs($Expected - $Actual) -gt 0.000001) { throw "$Message Expected=$Expected Actual=$Actual" }
|
||||
}
|
||||
|
||||
function Find-Method($Type, [string]$Name, [Type[]]$ParameterTypes) {
|
||||
foreach ($candidate in $Type.GetMethods()) {
|
||||
if ($candidate.Name -ne $Name) { continue }
|
||||
$parameters = $candidate.GetParameters()
|
||||
if ($parameters.Length -ne $ParameterTypes.Length) { continue }
|
||||
$matches = $true
|
||||
for ($index = 0; $index -lt $parameters.Length; $index++) {
|
||||
if ($parameters[$index].ParameterType -ne $ParameterTypes[$index]) {
|
||||
$matches = $false
|
||||
break
|
||||
}
|
||||
}
|
||||
if ($matches) { return $candidate }
|
||||
}
|
||||
return $null
|
||||
}
|
||||
|
||||
function New-EmptyPlanningMap {
|
||||
$mapping = 'MultiWheelC.TrajectoryPlanning.Mapping.'
|
||||
$boundsType = $assembly.GetType($mapping + 'MapBoundsMm', $true)
|
||||
$requestType = $assembly.GetType($mapping + 'PlanningMapRequest', $true)
|
||||
$factoryType = $assembly.GetType($mapping + 'PlanningMapFactory', $true)
|
||||
$request = [Activator]::CreateInstance($requestType)
|
||||
$request.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000))
|
||||
$request.ResolutionMm = [single]50
|
||||
$request.AllowExplicitEmptyMap = $true
|
||||
$result = [Activator]::CreateInstance($factoryType).Create($request)
|
||||
Assert-True $result.Succeeded 'Empty integration map must be created.'
|
||||
Assert-True $result.Map.PlanningReady 'Explicit empty integration map must be ready.'
|
||||
return $result.Map
|
||||
}
|
||||
|
||||
$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
|
||||
$plannerType = $assembly.GetType($coarsePath + 'HybridAStarPlanner', $true)
|
||||
$planner = [Activator]::CreateInstance($plannerType)
|
||||
$requestType = $assembly.GetType($coarsePath + 'PlanningRequest', $true)
|
||||
$poseType = $assembly.GetType($coarsePath + 'Pose2D', $true)
|
||||
$vehicleType = $assembly.GetType($coarsePath + 'VehicleParameters', $true)
|
||||
$configurationType = $assembly.GetType($coarsePath + 'HybridAStarConfiguration', $true)
|
||||
$goalDirectionType = $assembly.GetType($coarsePath + 'GoalDirectionConstraint', $true)
|
||||
$plan = Find-Method $plannerType 'Plan' @($requestType, [Threading.CancellationToken])
|
||||
Assert-True ($plan -ne $null) 'HybridAStarPlanner must expose Plan(request, cancellationToken).'
|
||||
|
||||
$defaultConfiguration = [Activator]::CreateInstance($configurationType)
|
||||
Assert-Near 0.15 $defaultConfiguration.GoalPositionToleranceMeters 'Default goal-position tolerance must retain the documented 0.15 m terminal acceptance radius.'
|
||||
|
||||
$request = [Activator]::CreateInstance($requestType)
|
||||
$request.Map = New-EmptyPlanningMap
|
||||
$request.Start = [Activator]::CreateInstance($poseType, @(1.0, 1.0, 0.0))
|
||||
$request.Goal = [Activator]::CreateInstance($poseType, @(1.20, 1.0, 0.0))
|
||||
$request.Vehicle = [Activator]::CreateInstance($vehicleType)
|
||||
$request.Vehicle.LengthMeters = 0.20
|
||||
$request.Vehicle.WidthMeters = 0.20
|
||||
$request.Vehicle.SafetyMarginMeters = 0.0
|
||||
$request.Vehicle.MaximumCurvaturePerMeter = 1.0
|
||||
$request.Configuration = [Activator]::CreateInstance($configurationType)
|
||||
$request.Configuration.MaximumExpandedNodes = 10000
|
||||
$request.Configuration.SearchTimeout = [TimeSpan]::FromSeconds(2.0)
|
||||
$request.Configuration.GoalPositionToleranceMeters = 0.001
|
||||
$request.Configuration.GoalHeadingToleranceRadians = 0.001
|
||||
$request.Configuration.AllowReverse = $false
|
||||
$request.GoalDirection = [Enum]::Parse($goalDirectionType, 'Forward')
|
||||
$request.StartVehicleCurvature = [double]0.20
|
||||
|
||||
$result = $plan.Invoke($planner, @($request, [Threading.CancellationToken]::None))
|
||||
Assert-Equal 'Success' $result.Status.ToString() 'Planner must publish a validated direct path on an empty map.'
|
||||
Assert-True ($result.Path.Count -ge 2) 'Validated path must retain its start and goal points.'
|
||||
Assert-Near 0.0 $result.Path[0].ArcLength 'The first path point must have zero arc length.'
|
||||
Assert-Near 0.20 $result.Path[0].VehicleCurvature `
|
||||
'The assembled start point must retain the requested physical vehicle curvature.'
|
||||
Assert-Equal 'Start' $result.Path[0].Source.ToString() 'The first path point must retain the start source.'
|
||||
Assert-Equal 'GoalTruncation' $result.Path[-1].Source.ToString() 'The terminal truncated point must retain its source.'
|
||||
Assert-True ($result.Diagnostics.PeakOpenListCount -ge 1) 'Planner diagnostics must retain the actual Open List peak count.'
|
||||
Assert-True ($result.Diagnostics.StaleOpenListEntryCount -ge 0) 'Planner diagnostics must retain a non-negative stale Open List count.'
|
||||
Assert-True ($result.Diagnostics.GetType().GetProperty('PathSearchElapsed') -ne $null) 'Planner diagnostics must expose path-search elapsed time.'
|
||||
Assert-True ($result.Diagnostics.PathSearchElapsed -ge [TimeSpan]::Zero) 'Successful planning must retain non-negative path-search time.'
|
||||
Assert-True ($result.Diagnostics.PathSearchElapsed -le $result.Diagnostics.Elapsed) 'Path-search time must not exceed total elapsed time.'
|
||||
|
||||
$nodeLimitedRequest = [Activator]::CreateInstance($requestType)
|
||||
$nodeLimitedRequest.Map = $request.Map
|
||||
$nodeLimitedRequest.Start = $request.Start
|
||||
$nodeLimitedRequest.Goal = $request.Goal
|
||||
$nodeLimitedRequest.Vehicle = $request.Vehicle
|
||||
$nodeLimitedRequest.Configuration = [Activator]::CreateInstance($configurationType)
|
||||
$nodeLimitedRequest.Configuration.MaximumExpandedNodes = 0
|
||||
$nodeLimitedRequest.Configuration.SearchTimeout = [TimeSpan]::FromSeconds(2)
|
||||
$nodeLimitedRequest.Configuration.GoalPositionToleranceMeters = 0.001
|
||||
$nodeLimitedRequest.Configuration.GoalHeadingToleranceRadians = 0.001
|
||||
$nodeLimitedRequest.Configuration.AllowReverse = $false
|
||||
$nodeLimitedRequest.GoalDirection = [Enum]::Parse($goalDirectionType, 'Forward')
|
||||
$nodeLimitedResult = $plan.Invoke($planner, @($nodeLimitedRequest, [Threading.CancellationToken]::None))
|
||||
Assert-Equal 'SearchNodeLimitExceeded' $nodeLimitedResult.Status.ToString() 'A zero node limit must fail after planner preflight.'
|
||||
Assert-True ($nodeLimitedResult.Diagnostics.PathSearchElapsed -ge [TimeSpan]::Zero) 'Search-stage node-limit failure must retain path-search time.'
|
||||
Assert-True ($nodeLimitedResult.Diagnostics.PathSearchElapsed -le $nodeLimitedResult.Diagnostics.Elapsed) 'Failed path-search time must not exceed total elapsed time.'
|
||||
Assert-True (-not [string]::IsNullOrWhiteSpace($nodeLimitedResult.Diagnostics.TerminationReason)) 'Planner diagnostics must retain a node-limit reason.'
|
||||
|
||||
for ($index = 1; $index -lt $result.Path.Count; $index++) {
|
||||
$previous = $result.Path[$index - 1]
|
||||
$current = $result.Path[$index]
|
||||
Assert-True ($current.ArcLength -ge $previous.ArcLength) 'Path arc length must be non-decreasing.'
|
||||
Assert-True ([Math]::Abs($current.UnwrappedHeading - $previous.UnwrappedHeading) -le ([Math]::PI + 0.000001)) 'Unwrapped heading must remain continuous between adjacent points.'
|
||||
$samePose = ([Math]::Abs($current.X - $previous.X) -le 0.000001) -and
|
||||
([Math]::Abs($current.Y - $previous.Y) -le 0.000001) -and
|
||||
([Math]::Abs($current.Heading - $previous.Heading) -le 0.000001) -and
|
||||
([Math]::Abs($current.ArcLength - $previous.ArcLength) -le 0.000001)
|
||||
if ($samePose) {
|
||||
Assert-True (($current.Direction -ne $previous.Direction) -and $current.IsGearSwitchPoint) 'Adjacent duplicate points are permitted only as a marked gear-switch pair.'
|
||||
}
|
||||
}
|
||||
|
||||
Assert-True ($result.Segments.Count -ge 1) 'Validated path must include inclusive direction segments.'
|
||||
Assert-Equal 0 $result.Segments[0].StartIndex 'The first segment must include the first path point.'
|
||||
Assert-Equal ($result.Path.Count - 1) $result.Segments[-1].EndIndex 'The final segment must include the final path point.'
|
||||
for ($index = 0; $index -lt $result.Segments.Count; $index++) {
|
||||
$segment = $result.Segments[$index]
|
||||
Assert-Equal $index $segment.SegmentIndex 'Segment indexes must be contiguous.'
|
||||
Assert-True ($segment.StartIndex -le $segment.EndIndex) 'Each segment must use inclusive ordered indexes.'
|
||||
if ($index -gt 0) {
|
||||
$previousSegment = $result.Segments[$index - 1]
|
||||
Assert-Equal ($previousSegment.EndIndex + 1) $segment.StartIndex 'Segments must cover each path point exactly once.'
|
||||
}
|
||||
}
|
||||
|
||||
Write-Output 'Coarse path integration checks passed.'
|
||||
|
||||
# Facade checks intentionally use only ASCII text so the script can also run in legacy PowerShell hosts.
|
||||
$facade = $coarsePath + 'Facade.'
|
||||
$serviceType = $assembly.GetType($facade + 'CoarsePathPlanningService', $true)
|
||||
$jobType = $assembly.GetType($facade + 'CoarsePathPlanningJob', $true)
|
||||
$jobResultType = $assembly.GetType($facade + 'CoarsePathPlanningJobResult', $true)
|
||||
$debugOptionsType = $assembly.GetType($facade + 'PlanningDebugOptions', $true)
|
||||
$debugSinkType = $assembly.GetType($facade + 'IPlanningDebugSink', $true)
|
||||
$mapRequestType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapRequest', $true)
|
||||
$boundsType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.MapBoundsMm', $true)
|
||||
$servicePlan = Find-Method $serviceType 'Plan' @($jobType, [Threading.CancellationToken])
|
||||
Assert-True ($servicePlan -ne $null) 'CoarsePathPlanningService must expose Plan(job, cancellationToken).'
|
||||
Assert-True ($jobResultType.GetProperty('MapResult') -ne $null) 'Facade result must retain the map result.'
|
||||
Assert-True ($jobResultType.GetProperty('PlanningResult') -ne $null) 'Facade result must retain the planning result.'
|
||||
Assert-True ($jobResultType.GetProperty('DebugDiagnostics') -ne $null) 'Facade result must retain debug diagnostics.'
|
||||
|
||||
function New-FacadeJob {
|
||||
$job = [Activator]::CreateInstance($jobType)
|
||||
$mapRequest = [Activator]::CreateInstance($mapRequestType)
|
||||
$mapRequest.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000))
|
||||
$mapRequest.ResolutionMm = [single]50
|
||||
$mapRequest.AllowExplicitEmptyMap = $true
|
||||
$job.MapRequest = $mapRequest
|
||||
$job.Start = [Activator]::CreateInstance($poseType, @(1.0, 1.0, 0.0))
|
||||
$job.Goal = [Activator]::CreateInstance($poseType, @(1.20, 1.0, 0.0))
|
||||
$job.Vehicle = [Activator]::CreateInstance($vehicleType)
|
||||
$job.Vehicle.LengthMeters = 0.20
|
||||
$job.Vehicle.WidthMeters = 0.20
|
||||
$job.Vehicle.SafetyMarginMeters = 0.0
|
||||
$job.Vehicle.MaximumCurvaturePerMeter = 1.0
|
||||
$job.Configuration = [Activator]::CreateInstance($configurationType)
|
||||
$job.Configuration.MaximumExpandedNodes = 10000
|
||||
$job.Configuration.SearchTimeout = [TimeSpan]::FromSeconds(2.0)
|
||||
$job.Configuration.GoalPositionToleranceMeters = 0.001
|
||||
$job.Configuration.GoalHeadingToleranceRadians = 0.001
|
||||
$job.Configuration.AllowReverse = $false
|
||||
$job.GoalDirection = [Enum]::Parse($goalDirectionType, 'Forward')
|
||||
return $job
|
||||
}
|
||||
|
||||
$service = [Activator]::CreateInstance($serviceType)
|
||||
$firstFacadeResult = $servicePlan.Invoke($service, @((New-FacadeJob), [Threading.CancellationToken]::None))
|
||||
Assert-True $firstFacadeResult.MapResult.Succeeded 'Facade must build the requested map first.'
|
||||
Assert-Equal 'Success' $firstFacadeResult.PlanningResult.Status.ToString() 'Facade must pass a successful map to the planner.'
|
||||
Assert-Equal 0 $firstFacadeResult.DebugDiagnostics.Count 'Default debug sink must not add diagnostics.'
|
||||
|
||||
$secondFacadeResult = $servicePlan.Invoke($service, @((New-FacadeJob), [Threading.CancellationToken]::None))
|
||||
Assert-True $secondFacadeResult.MapResult.Succeeded 'Repeated facade request must retain a map result.'
|
||||
Assert-Equal 'Input' $secondFacadeResult.MapResult.CacheHit.ToString() 'One facade service must retain its map factory cache.'
|
||||
Assert-Equal $firstFacadeResult.MapResult.Map.InputFingerprint $secondFacadeResult.MapResult.Map.InputFingerprint 'Map fingerprint must remain stable for equal input.'
|
||||
Assert-Equal $firstFacadeResult.PlanningResult.Status $secondFacadeResult.PlanningResult.Status 'Map cache reuse must not change planning status.'
|
||||
Assert-Equal $firstFacadeResult.PlanningResult.Path.Count $secondFacadeResult.PlanningResult.Path.Count 'Map cache reuse must not change path point count.'
|
||||
|
||||
$invalidJob = [Activator]::CreateInstance($jobType)
|
||||
$invalidJob.MapRequest = [Activator]::CreateInstance($mapRequestType)
|
||||
$mapFailureResult = $servicePlan.Invoke($service, @($invalidJob, [Threading.CancellationToken]::None))
|
||||
Assert-False $mapFailureResult.MapResult.Succeeded 'Invalid map input must be returned as a map failure.'
|
||||
Assert-Equal 'InvalidMap' $mapFailureResult.PlanningResult.Status.ToString() 'Map failure must return an empty non-search planning result.'
|
||||
Assert-Equal 0 $mapFailureResult.PlanningResult.Diagnostics.ExpandedNodeCount 'Map failure must not start the search.'
|
||||
Assert-Equal 0 $mapFailureResult.PlanningResult.Path.Count 'Map failure must not publish a path.'
|
||||
Assert-Equal ([TimeSpan]::Zero) $mapFailureResult.PlanningResult.Diagnostics.PathSearchElapsed 'Map failure must report zero path-search time.'
|
||||
|
||||
$cancelledFacadeSource = New-Object Threading.CancellationTokenSource
|
||||
$cancelledFacadeSource.Cancel()
|
||||
$cancelledFacadeResult = $servicePlan.Invoke($service, @((New-FacadeJob), $cancelledFacadeSource.Token))
|
||||
Assert-Equal 'Cancelled' $cancelledFacadeResult.MapResult.Status.ToString() 'Facade must retain a cancelled map result.'
|
||||
Assert-Equal 'Cancelled' $cancelledFacadeResult.PlanningResult.Status.ToString() 'Facade must map map-stage cancellation to planning cancellation.'
|
||||
Assert-Equal 0 $cancelledFacadeResult.PlanningResult.Path.Count 'Cancelled facade planning must publish no path.'
|
||||
Assert-Equal 0 $cancelledFacadeResult.PlanningResult.Segments.Count 'Cancelled facade planning must publish no segments.'
|
||||
|
||||
$timedOutFacadeJob = New-FacadeJob
|
||||
$timedOutFacadeJob.Configuration.SearchTimeout = [TimeSpan]::Zero
|
||||
$timedOutFacadeResult = $servicePlan.Invoke($service, @($timedOutFacadeJob, [Threading.CancellationToken]::None))
|
||||
Assert-Equal 'TimedOut' $timedOutFacadeResult.MapResult.Status.ToString() 'Facade total timeout must stop before publishing a map.'
|
||||
Assert-Equal 'SearchTimeout' $timedOutFacadeResult.PlanningResult.Status.ToString() 'Facade must map map-stage timeout to planning timeout.'
|
||||
Assert-Equal 0 $timedOutFacadeResult.PlanningResult.Path.Count 'Timed out facade planning must publish no path.'
|
||||
|
||||
$defaultDebugOptions = [Activator]::CreateInstance($debugOptionsType)
|
||||
Assert-False $defaultDebugOptions.Enabled 'Debug output must be opt-in.'
|
||||
Assert-True ($defaultDebugOptions.Sink -ne $null) 'Debug options must default to a no-op sink.'
|
||||
|
||||
$debugSinkSource = @'
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
|
||||
public sealed class FacadeRecordingDebugSink : IPlanningDebugSink
|
||||
{
|
||||
public int PublishCount { get; private set; }
|
||||
public void Publish(PlanningMapBuildResult mapResult, PlanningResult planningResult)
|
||||
{
|
||||
PublishCount++;
|
||||
}
|
||||
}
|
||||
|
||||
public sealed class FacadeThrowingDebugSink : IPlanningDebugSink
|
||||
{
|
||||
public void Publish(PlanningMapBuildResult mapResult, PlanningResult planningResult)
|
||||
{
|
||||
throw new InvalidOperationException("debug sink failure");
|
||||
}
|
||||
}
|
||||
'@
|
||||
$runtimeDirectory = [Runtime.InteropServices.RuntimeEnvironment]::GetRuntimeDirectory()
|
||||
$debugSinkReferences = @(
|
||||
(Join-Path $runtimeDirectory 'mscorlib.dll'),
|
||||
(Join-Path $runtimeDirectory 'System.dll'),
|
||||
(Join-Path $runtimeDirectory 'System.Core.dll'),
|
||||
$AssemblyPath,
|
||||
'C:\Program Files (x86)\Reference Assemblies\Microsoft\Framework\.NETFramework\v4.8\Facades\netstandard.dll'
|
||||
)
|
||||
Add-Type -TypeDefinition $debugSinkSource -ReferencedAssemblies $debugSinkReferences
|
||||
$recordingSinkType = 'FacadeRecordingDebugSink' -as [type]
|
||||
$throwingSinkType = 'FacadeThrowingDebugSink' -as [type]
|
||||
|
||||
$disabledDebugOptions = [Activator]::CreateInstance($debugOptionsType)
|
||||
$recordingSink = [Activator]::CreateInstance($recordingSinkType)
|
||||
$disabledDebugOptions.Sink = $recordingSink
|
||||
$disabledJob = New-FacadeJob
|
||||
$disabledJob.DebugOptions = $disabledDebugOptions
|
||||
$disabledResult = $servicePlan.Invoke($service, @($disabledJob, [Threading.CancellationToken]::None))
|
||||
Assert-Equal 0 $recordingSink.PublishCount 'Disabled debug output must not publish to the sink.'
|
||||
Assert-Equal $firstFacadeResult.PlanningResult.Status $disabledResult.PlanningResult.Status 'Disabled debug output must not change planning status.'
|
||||
|
||||
$throwingDebugOptions = [Activator]::CreateInstance($debugOptionsType)
|
||||
$throwingDebugOptions.Enabled = $true
|
||||
$throwingDebugOptions.Sink = [Activator]::CreateInstance($throwingSinkType)
|
||||
$throwingJob = New-FacadeJob
|
||||
$throwingJob.DebugOptions = $throwingDebugOptions
|
||||
$throwingResult = $servicePlan.Invoke($service, @($throwingJob, [Threading.CancellationToken]::None))
|
||||
Assert-Equal $firstFacadeResult.MapResult.Map.InputFingerprint $throwingResult.MapResult.Map.InputFingerprint 'Debug sink errors must not change the map fingerprint.'
|
||||
Assert-Equal $firstFacadeResult.PlanningResult.Status $throwingResult.PlanningResult.Status 'Debug sink errors must not change planning status.'
|
||||
Assert-Equal $firstFacadeResult.PlanningResult.Path.Count $throwingResult.PlanningResult.Path.Count 'Debug sink errors must not change the path.'
|
||||
Assert-Equal 1 $throwingResult.DebugDiagnostics.Count 'Debug sink errors must be retained as diagnostics.'
|
||||
|
||||
Write-Output 'Coarse path facade checks passed.'
|
||||
|
||||
# P1 UI scenario factory contract checks. These run against the same built assembly
|
||||
# and intentionally fail until the factory is introduced.
|
||||
$testNamespace = $coarsePath + 'Test.'
|
||||
$scenarioEnumType = $assembly.GetType($testNamespace + 'CoarsePathTestScenario', $false)
|
||||
$scenarioFactoryType = $assembly.GetType($testNamespace + 'CoarsePathScenarioFactory', $false)
|
||||
Assert-True ($scenarioEnumType -ne $null) 'P1 scenario enum must exist.'
|
||||
Assert-True ($scenarioFactoryType -ne $null) 'P1 scenario factory must exist.'
|
||||
|
||||
$factoryCreate = Find-Method $scenarioFactoryType 'Create' @($scenarioEnumType)
|
||||
$factoryCreateAtAmr = Find-Method $scenarioFactoryType 'Create' @(
|
||||
$scenarioEnumType, [double], [double], [double])
|
||||
$factoryManual = Find-Method $scenarioFactoryType 'CreateManualGoalDemo' @(
|
||||
[double], [double], [double], [double], [double], [double])
|
||||
Assert-True ($factoryCreate -ne $null) 'P1 scenario factory must expose Create(scenario).'
|
||||
Assert-True ($factoryCreateAtAmr -ne $null) 'Scenario factory must expose Create(scenario, amrX, amrY, amrHeading).'
|
||||
Assert-True ($factoryManual -ne $null) 'P1 scenario factory must expose CreateManualGoalDemo with six doubles.'
|
||||
|
||||
$scenarioNames = @('ExplicitEmpty', 'RectangleDetour', 'ManualAndTwoLeg', 'CacheHit', 'ReverseGearSwitch', 'NoFeasiblePath')
|
||||
foreach ($scenarioName in $scenarioNames) {
|
||||
$scenario = [Enum]::Parse($scenarioEnumType, $scenarioName)
|
||||
$jobA = $factoryCreate.Invoke($null, @($scenario))
|
||||
$jobB = $factoryCreate.Invoke($null, @($scenario))
|
||||
Assert-True ($jobA -ne $null) "Scenario $scenarioName must return a job."
|
||||
Assert-False ([object]::ReferenceEquals($jobA, $jobB)) "Scenario $scenarioName must return a new job per call."
|
||||
}
|
||||
|
||||
foreach ($scenarioName in $scenarioNames) {
|
||||
$scenario = [Enum]::Parse($scenarioEnumType, $scenarioName)
|
||||
$liveJob = $factoryCreateAtAmr.Invoke($null, @($scenario, 12345.0, -6789.0, 135.0))
|
||||
Assert-Near 12.345 $liveJob.Start.X "Live $scenarioName start X must equal the AMR X."
|
||||
Assert-Near -6.789 $liveJob.Start.Y "Live $scenarioName start Y must equal the AMR Y."
|
||||
Assert-Near (3.0 * [Math]::PI / 4.0) $liveJob.Start.Heading "Live $scenarioName heading must equal the AMR heading."
|
||||
}
|
||||
|
||||
$rectangleScenario = [Enum]::Parse($scenarioEnumType, 'RectangleDetour')
|
||||
$liveRectangle = $factoryCreateAtAmr.Invoke($null, @($rectangleScenario, 12000.0, -3000.0, 90.0))
|
||||
Assert-Near 12.0 $liveRectangle.Start.X 'Live rectangle start X must equal the AMR X.'
|
||||
Assert-Near -3.0 $liveRectangle.Start.Y 'Live rectangle start Y must equal the AMR Y.'
|
||||
Assert-Near ([Math]::PI / 2.0) $liveRectangle.Start.Heading 'Live rectangle start heading must equal the AMR heading.'
|
||||
Assert-Near 16.0 $liveRectangle.Goal.X 'Live rectangle goal X must preserve the four-metre relative offset.'
|
||||
Assert-Near -3.0 $liveRectangle.Goal.Y 'Live rectangle goal Y must preserve the relative offset.'
|
||||
Assert-Near ([Math]::PI / 2.0) $liveRectangle.Goal.Heading 'Live rectangle goal heading must preserve the zero baseline heading delta.'
|
||||
Assert-Near 11000.0 $liveRectangle.MapRequest.Bounds.XMin 'Live rectangle map X minimum must translate with the AMR.'
|
||||
Assert-Near -1000.0 $liveRectangle.MapRequest.Bounds.YMax 'Live rectangle map Y maximum must translate with the AMR.'
|
||||
$rectangleProjection = $liveRectangle.MapRequest.ObstacleSources[0].ProjectToWorld()
|
||||
$rectangleObstacle = $rectangleProjection.Obstacles[0]
|
||||
Assert-Near 13700.0 $rectangleObstacle.XMin 'Live rectangle obstacle X minimum must translate with the AMR.'
|
||||
Assert-Near -2200.0 $rectangleObstacle.YMax 'Live rectangle obstacle Y maximum must translate with the AMR.'
|
||||
|
||||
$multiScenario = [Enum]::Parse($scenarioEnumType, 'ManualAndTwoLeg')
|
||||
$baselineMulti = $factoryCreate.Invoke($null, @($multiScenario))
|
||||
$liveMulti = $factoryCreateAtAmr.Invoke($null, @($multiScenario, 7000.0, 8000.0, 45.0))
|
||||
Assert-Near 7.0 $liveMulti.Start.X 'Live multi-source start X must equal AMR X.'
|
||||
Assert-Near 8.0 $liveMulti.Start.Y 'Live multi-source start Y must equal AMR Y.'
|
||||
Assert-Near ([Math]::PI / 4.0) $liveMulti.Goal.Heading 'Live multi-source goal heading must follow AMR heading.'
|
||||
$baselineTwoLeg = $baselineMulti.MapRequest.ObstacleSources[1].ProjectToWorld().Obstacles
|
||||
$liveTwoLeg = $liveMulti.MapRequest.ObstacleSources[1].ProjectToWorld().Obstacles
|
||||
Assert-Near ($baselineTwoLeg[0].CenterX + 6000.0) $liveTwoLeg[0].CenterX 'TwoLeg X must translate without rotation.'
|
||||
Assert-Near ($baselineTwoLeg[0].CenterY + 7000.0) $liveTwoLeg[0].CenterY 'TwoLeg Y must translate without rotation.'
|
||||
|
||||
$noPathScenario = [Enum]::Parse($scenarioEnumType, 'NoFeasiblePath')
|
||||
$liveNoPath = $factoryCreateAtAmr.Invoke($null, @($noPathScenario, 9000.0, -1000.0, -180.0))
|
||||
Assert-Near 9.0 $liveNoPath.Start.X 'Live infeasible scenario must use AMR X.'
|
||||
Assert-Near -1.0 $liveNoPath.Start.Y 'Live infeasible scenario must use AMR Y.'
|
||||
Assert-Near 8000.0 $liveNoPath.MapRequest.Bounds.XMin 'Live infeasible map must translate with its baseline start.'
|
||||
|
||||
$invalidLivePoseRejected = $false
|
||||
try { $null = $factoryCreateAtAmr.Invoke($null, @($rectangleScenario, [double]::NaN, 0.0, 0.0)) }
|
||||
catch [ArgumentOutOfRangeException] {
|
||||
$invalidLivePoseRejected = $true
|
||||
}
|
||||
catch [Reflection.TargetInvocationException] {
|
||||
$invalidLivePoseRejected = $_.Exception.InnerException -is [ArgumentOutOfRangeException]
|
||||
}
|
||||
Assert-True $invalidLivePoseRejected 'Live factory must reject non-finite AMR coordinates.'
|
||||
|
||||
$manualJob = $factoryManual.Invoke($null, @(1000.0, 2000.0, 90.0, 4000.0, 2000.0, 0.0))
|
||||
Assert-Near 1.0 $manualJob.Start.X 'Manual AMR X must convert mm to m.'
|
||||
Assert-Near 2.0 $manualJob.Start.Y 'Manual AMR Y must convert mm to m.'
|
||||
Assert-Near ([Math]::PI / 2.0) $manualJob.Start.Heading 'Manual AMR heading must convert degrees to radians.'
|
||||
Assert-Near 4.0 $manualJob.Goal.X 'Manual goal X must convert mm to m.'
|
||||
Assert-Near 0.0 $manualJob.Goal.Heading 'Manual goal heading must convert degrees to radians.'
|
||||
Assert-True $manualJob.MapRequest.AllowExplicitEmptyMap 'Manual goal demo must declare its empty map explicitly.'
|
||||
|
||||
$slowFeasibleJob = $factoryManual.Invoke($null, @(1000.0, 2000.0, 0.0, 1500.0, 2500.0, 90.0))
|
||||
$slowFeasibleJob.Configuration.SearchTimeout = [TimeSpan]::FromSeconds(30)
|
||||
$slowFeasibleJob.Configuration.MaximumExpandedNodes = 1000000
|
||||
$slowFeasibleService = [Activator]::CreateInstance($serviceType)
|
||||
$slowFeasibleResult = $servicePlan.Invoke($slowFeasibleService, @($slowFeasibleJob, [Threading.CancellationToken]::None))
|
||||
Assert-Equal 'Success' $slowFeasibleResult.PlanningResult.Status.ToString() 'The previously five-second-limited feasible pose must succeed with a caller-selected longer budget.'
|
||||
Assert-True ($slowFeasibleResult.PlanningResult.Diagnostics.PathSearchElapsed -le $slowFeasibleResult.PlanningResult.Diagnostics.Elapsed) 'Slow feasible path-search time must remain within total elapsed time.'
|
||||
|
||||
$manualObstacleKindType = $assembly.GetType($testNamespace + 'ManualCoarsePathObstacleKind', $false)
|
||||
$manualObstacleType = $assembly.GetType($testNamespace + 'ManualCoarsePathObstacle', $false)
|
||||
Assert-True ($manualObstacleKindType -ne $null) 'Manual obstacle kind enum must exist.'
|
||||
Assert-True ($manualObstacleType -ne $null) 'Manual obstacle value type must exist.'
|
||||
|
||||
$manualCircle = Find-Method $manualObstacleType 'Circle' @([double], [double], [double])
|
||||
$manualRectangle = Find-Method $manualObstacleType 'AxisAlignedRectangle' @([double], [double], [double], [double])
|
||||
$manualObstacleFactory = $scenarioFactoryType.GetMethods() | Where-Object {
|
||||
$_.Name -eq 'CreateManualObstacleDemo' -and $_.GetParameters().Length -eq 8
|
||||
} | Select-Object -First 1
|
||||
Assert-True ($manualCircle -ne $null) 'Manual obstacle type must create circles from center and radius.'
|
||||
Assert-True ($manualRectangle -ne $null) 'Manual obstacle type must create rectangles from center and X/Y dimensions.'
|
||||
Assert-True ($manualObstacleFactory -ne $null) 'Scenario factory must expose CreateManualObstacleDemo with six poses, obstacles and version.'
|
||||
|
||||
$manualObstacles = [Array]::CreateInstance($manualObstacleType, 2)
|
||||
$manualObstacles.SetValue($manualCircle.Invoke($null, @([double]6500, [double]2000, [double]200)), 0)
|
||||
$manualObstacles.SetValue($manualRectangle.Invoke($null, @([double]-2000, [double]500, [double]600, [double]400)), 1)
|
||||
$manualObstacleJob = $manualObstacleFactory.Invoke($null, @(
|
||||
1000.0, 2000.0, 0.0, 4000.0, 2000.0, 0.0, $manualObstacles, [long]77))
|
||||
Assert-False $manualObstacleJob.MapRequest.AllowExplicitEmptyMap 'Manual obstacles must disable the explicit-empty-map mode.'
|
||||
Assert-Equal 1 $manualObstacleJob.MapRequest.ObstacleSources.Count 'Manual obstacles must create one unified source.'
|
||||
Assert-Equal 'manual-user-input' $manualObstacleJob.MapRequest.ObstacleSources[0].SourceId 'Manual source ID must be stable.'
|
||||
Assert-Equal 77 $manualObstacleJob.MapRequest.ObstacleSources[0].SourceVersion 'Manual source version must be preserved.'
|
||||
Assert-True ($manualObstacleJob.MapRequest.Bounds.XMin -le -4300.0) 'Manual map must include the rectangle outline and padding.'
|
||||
Assert-True ($manualObstacleJob.MapRequest.Bounds.XMax -ge 8700.0) 'Manual map must include the circle outline and padding.'
|
||||
|
||||
$emptyManualObstacles = [Array]::CreateInstance($manualObstacleType, 0)
|
||||
$emptyManualJob = $manualObstacleFactory.Invoke($null, @(
|
||||
1000.0, 2000.0, 0.0, 4000.0, 2000.0, 0.0, $emptyManualObstacles, [long]0))
|
||||
Assert-True $emptyManualJob.MapRequest.AllowExplicitEmptyMap 'Zero manual obstacles must retain explicit empty-map mode.'
|
||||
Assert-Equal 0 $emptyManualJob.MapRequest.ObstacleSources.Count 'Zero manual obstacles must not create a fake source.'
|
||||
|
||||
$invalidGeometryRejected = $false
|
||||
try {
|
||||
$null = $manualCircle.Invoke($null, @([double]1000, [double]1000, [double]0))
|
||||
}
|
||||
catch [ArgumentOutOfRangeException] { $invalidGeometryRejected = $true }
|
||||
catch [Reflection.TargetInvocationException] {
|
||||
$invalidGeometryRejected = $_.Exception.InnerException -is [ArgumentOutOfRangeException]
|
||||
}
|
||||
Assert-True $invalidGeometryRejected 'Invalid manual geometry must report argument range.'
|
||||
|
||||
$scenarioService = [Activator]::CreateInstance($serviceType)
|
||||
foreach ($scenarioName in @('ExplicitEmpty', 'RectangleDetour', 'ManualAndTwoLeg', 'ReverseGearSwitch')) {
|
||||
$job = $factoryCreate.Invoke($null, @([Enum]::Parse($scenarioEnumType, $scenarioName)))
|
||||
$result = $servicePlan.Invoke($scenarioService, @($job, [Threading.CancellationToken]::None))
|
||||
Assert-Equal 'Success' $result.PlanningResult.Status.ToString() "Scenario $scenarioName must succeed."
|
||||
}
|
||||
|
||||
$reverseJob = $factoryCreate.Invoke($null, @([Enum]::Parse($scenarioEnumType, 'ReverseGearSwitch')))
|
||||
$reverseResult = $servicePlan.Invoke($scenarioService, @($reverseJob, [Threading.CancellationToken]::None))
|
||||
Assert-True (($reverseResult.PlanningResult.Path | Where-Object { $_.IsGearSwitchPoint }).Count -ge 1) 'Reverse scenario must expose a gear-switch point.'
|
||||
|
||||
$noPathJob = $factoryCreate.Invoke($null, @([Enum]::Parse($scenarioEnumType, 'NoFeasiblePath')))
|
||||
$noPathResult = $servicePlan.Invoke($scenarioService, @($noPathJob, [Threading.CancellationToken]::None))
|
||||
Assert-Equal 'NoFeasiblePath' $noPathResult.PlanningResult.Status.ToString() 'Barrier scenario must be infeasible.'
|
||||
Assert-Equal 0 $noPathResult.PlanningResult.Path.Count 'Infeasible scenario must not publish a path.'
|
||||
Assert-True ($noPathResult.PlanningResult.Diagnostics.TerminationReason.Contains('Open List')) 'No-path diagnostics must retain the exact search exhaustion reason.'
|
||||
Assert-True (-not [string]::IsNullOrWhiteSpace($noPathResult.PlanningResult.Diagnostics.TerminationReason)) 'No-path diagnostics must retain a reason.'
|
||||
|
||||
$cacheJobA = $factoryCreate.Invoke($null, @([Enum]::Parse($scenarioEnumType, 'CacheHit')))
|
||||
$cacheJobB = $factoryCreate.Invoke($null, @([Enum]::Parse($scenarioEnumType, 'CacheHit')))
|
||||
$cacheFirst = $servicePlan.Invoke($scenarioService, @($cacheJobA, [Threading.CancellationToken]::None))
|
||||
$cacheSecond = $servicePlan.Invoke($scenarioService, @($cacheJobB, [Threading.CancellationToken]::None))
|
||||
Assert-Equal 'Input' $cacheSecond.MapResult.CacheHit.ToString() 'Cache-hit scenario must reuse the complete map input.'
|
||||
Assert-Equal $cacheFirst.PlanningResult.Status $cacheSecond.PlanningResult.Status 'Map cache reuse must not change planning status.'
|
||||
|
||||
Write-Output 'Coarse path P1 scenario checks passed.'
|
||||
@@ -0,0 +1,411 @@
|
||||
param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
|
||||
$ErrorActionPreference = 'Stop'
|
||||
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
|
||||
|
||||
function Assert-True($Actual, [string]$Message) {
|
||||
if (-not $Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-Null($Actual, [string]$Message) {
|
||||
if ($null -ne $Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-False($Actual, [string]$Message) {
|
||||
if ($Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-Near([double]$Expected, [double]$Actual, [string]$Message) {
|
||||
if ([Math]::Abs($Expected - $Actual) -gt 0.000001) { throw "$Message Expected=$Expected Actual=$Actual" }
|
||||
}
|
||||
|
||||
function Assert-Equal($Expected, $Actual, [string]$Message) {
|
||||
if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" }
|
||||
}
|
||||
|
||||
function Assert-Throws([scriptblock]$Action, [string]$Message) {
|
||||
try {
|
||||
& $Action
|
||||
}
|
||||
catch {
|
||||
return
|
||||
}
|
||||
|
||||
throw $Message
|
||||
}
|
||||
|
||||
function Find-Method($Type, [string]$Name, [Type[]]$ParameterTypes) {
|
||||
foreach ($candidate in $Type.GetMethods()) {
|
||||
if ($candidate.Name -ne $Name) { continue }
|
||||
$parameters = $candidate.GetParameters()
|
||||
if ($parameters.Length -ne $ParameterTypes.Length) { continue }
|
||||
$matches = $true
|
||||
for ($index = 0; $index -lt $parameters.Length; $index++) {
|
||||
if ($parameters[$index].ParameterType -ne $ParameterTypes[$index]) {
|
||||
$matches = $false
|
||||
break
|
||||
}
|
||||
}
|
||||
if ($matches) { return $candidate }
|
||||
}
|
||||
return $null
|
||||
}
|
||||
|
||||
function Find-NonPublicStaticMethod($Type, [string]$Name, [Type[]]$ParameterTypes) {
|
||||
foreach ($candidate in $Type.GetMethods([Reflection.BindingFlags]'Static,NonPublic')) {
|
||||
if ($candidate.Name -ne $Name) { continue }
|
||||
$parameters = $candidate.GetParameters()
|
||||
if ($parameters.Length -ne $ParameterTypes.Length) { continue }
|
||||
$matches = $true
|
||||
for ($index = 0; $index -lt $parameters.Length; $index++) {
|
||||
if ($parameters[$index].ParameterType -ne $ParameterTypes[$index]) { $matches = $false; break }
|
||||
}
|
||||
if ($matches) { return $candidate }
|
||||
}
|
||||
return $null
|
||||
}
|
||||
|
||||
function New-TestMap {
|
||||
$mapping = 'MultiWheelC.TrajectoryPlanning.Mapping.'
|
||||
$boundsType = $assembly.GetType($mapping + 'MapBoundsMm', $true)
|
||||
$requestType = $assembly.GetType($mapping + 'PlanningMapRequest', $true)
|
||||
$factoryType = $assembly.GetType($mapping + 'PlanningMapFactory', $true)
|
||||
$bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000))
|
||||
$request = [Activator]::CreateInstance($requestType)
|
||||
$request.Bounds = $bounds
|
||||
$request.ResolutionMm = [single]50
|
||||
$request.AllowExplicitEmptyMap = $true
|
||||
$result = [Activator]::CreateInstance($factoryType).Create($request)
|
||||
Assert-True $result.Succeeded 'Test map must be created.'
|
||||
Assert-True $result.Map.PlanningReady 'Test map must be ready.'
|
||||
return $result.Map
|
||||
}
|
||||
|
||||
function New-CornerBlockedMap {
|
||||
$mapping = 'MultiWheelC.TrajectoryPlanning.Mapping.'
|
||||
$boundsType = $assembly.GetType($mapping + 'MapBoundsMm', $true)
|
||||
$requestType = $assembly.GetType($mapping + 'PlanningMapRequest', $true)
|
||||
$factoryType = $assembly.GetType($mapping + 'PlanningMapFactory', $true)
|
||||
$obstacleType = $assembly.GetType($mapping + 'IMapObstacle', $true)
|
||||
$circleType = $assembly.GetType($mapping + 'CircleObstacle', $true)
|
||||
$manualType = $assembly.GetType($mapping + 'ManualObstacleSource', $true)
|
||||
$sourceType = $assembly.GetType($mapping + 'IMapObstacleSource', $true)
|
||||
$bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]150, [single]0, [single]150))
|
||||
$obstacles = [Array]::CreateInstance($obstacleType, 2)
|
||||
$obstacles.SetValue([Activator]::CreateInstance($circleType, @([single]75, [single]25, [single]0)), 0)
|
||||
$obstacles.SetValue([Activator]::CreateInstance($circleType, @([single]25, [single]75, [single]0)), 1)
|
||||
$sources = [Array]::CreateInstance($sourceType, 1)
|
||||
$sources.SetValue([Activator]::CreateInstance($manualType, @('corner-blocks', [long]1, $true, $obstacles)), 0)
|
||||
$request = [Activator]::CreateInstance($requestType)
|
||||
$request.Bounds = $bounds
|
||||
$request.ResolutionMm = [single]50
|
||||
$request.ObstacleSources = $sources
|
||||
$request.AllowExplicitEmptyMap = $false
|
||||
$result = [Activator]::CreateInstance($factoryType).Create($request)
|
||||
Assert-True $result.Succeeded 'Corner-blocked map must be created.'
|
||||
Assert-True $result.Map.PlanningReady 'Corner-blocked map must be ready.'
|
||||
return $result.Map
|
||||
}
|
||||
|
||||
$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
|
||||
$search = $coarsePath + 'Search.'
|
||||
$primitiveType = $assembly.GetType($search + 'MotionPrimitive', $true)
|
||||
$generatorType = $assembly.GetType($search + 'MotionPrimitiveGenerator', $true)
|
||||
$goalCheckerType = $assembly.GetType($search + 'GoalToleranceChecker', $true)
|
||||
|
||||
Assert-True ($primitiveType.GetProperty('ActualLengthMeters') -ne $null) 'MotionPrimitive must expose actual length.'
|
||||
Assert-True ($primitiveType.GetProperty('Points') -ne $null) 'MotionPrimitive must expose integration points.'
|
||||
Assert-True ($primitiveType.GetProperty('IsGoalTruncation') -ne $null) 'MotionPrimitive must expose goal truncation state.'
|
||||
|
||||
$poseType = $assembly.GetType($coarsePath + 'Pose2D', $true)
|
||||
$requestType = $assembly.GetType($coarsePath + 'PlanningRequest', $true)
|
||||
$configurationType = $assembly.GetType($coarsePath + 'HybridAStarConfiguration', $true)
|
||||
$vehicleType = $assembly.GetType($coarsePath + 'VehicleParameters', $true)
|
||||
$directionType = $assembly.GetType($coarsePath + 'TravelDirection', $true)
|
||||
$goalDirectionType = $assembly.GetType($coarsePath + 'GoalDirectionConstraint', $true)
|
||||
$generate = Find-Method $generatorType 'Generate' @($poseType, [double], $directionType, $requestType)
|
||||
Assert-True ($generate -ne $null) 'MotionPrimitiveGenerator must expose Generate(start, curvature, direction, request).'
|
||||
$isSatisfied = Find-Method $goalCheckerType 'IsSatisfied' @($poseType, $poseType, $configurationType, $directionType, $goalDirectionType)
|
||||
Assert-True ($isSatisfied -ne $null) 'GoalToleranceChecker must expose the planned goal check.'
|
||||
|
||||
$map = New-TestMap
|
||||
$vehicle = [Activator]::CreateInstance($vehicleType)
|
||||
$vehicle.LengthMeters = 0.20
|
||||
$vehicle.WidthMeters = 0.20
|
||||
$vehicle.SafetyMarginMeters = 0.0
|
||||
$vehicle.MaximumCurvaturePerMeter = 1.0
|
||||
$config = [Activator]::CreateInstance($configurationType)
|
||||
$config.PrimitiveLengthMeters = 0.50
|
||||
$config.IntegrationStepMeters = 0.05
|
||||
$config.MaximumCollisionCheckStepMeters = 0.025
|
||||
$config.GoalPositionToleranceMeters = 0.001
|
||||
$config.GoalHeadingToleranceRadians = 0.001
|
||||
$config.CurvatureLevelCount = 5
|
||||
$forward = [Enum]::Parse($directionType, 'Forward')
|
||||
$reverse = [Enum]::Parse($directionType, 'Reverse')
|
||||
$anyDirection = [Enum]::Parse($goalDirectionType, 'Any')
|
||||
$forwardOnly = [Enum]::Parse($goalDirectionType, 'Forward')
|
||||
$start = [Activator]::CreateInstance($poseType, @(1.0, 1.0, 0.0))
|
||||
$generator = [Activator]::CreateInstance($generatorType)
|
||||
|
||||
$request = [Activator]::CreateInstance($requestType)
|
||||
$request.Map = $map
|
||||
$request.Vehicle = $vehicle
|
||||
$request.Configuration = $config
|
||||
$request.Goal = [Activator]::CreateInstance($poseType, @(3.0, 1.0, 0.0))
|
||||
$request.GoalDirection = $anyDirection
|
||||
$straight = $generate.Invoke($generator, @($start, 0.0, $forward, $request))
|
||||
Assert-True ($straight -ne $null) 'Straight primitive must be generated.'
|
||||
Assert-Near 0.50 $straight.ActualLengthMeters 'Straight primitive must use the configured cap.'
|
||||
Assert-Near 1.50 $straight.End.X 'Straight primitive must use analytic integration.'
|
||||
Assert-Near 1.00 $straight.End.Y 'Straight primitive must not drift laterally.'
|
||||
Assert-Near 0.00 $straight.End.Heading 'Straight primitive must keep heading.'
|
||||
Assert-True ($straight.Points.Count -ge 20) 'Point spacing must honor the collision check step.'
|
||||
$previous = $start
|
||||
foreach ($point in $straight.Points) {
|
||||
$distance = [Math]::Sqrt(($point.X - $previous.X) * ($point.X - $previous.X) + ($point.Y - $previous.Y) * ($point.Y - $previous.Y))
|
||||
Assert-True ($distance -le 0.025001) 'Point spacing must not exceed the map-safe collision step.'
|
||||
$previous = $point
|
||||
}
|
||||
|
||||
$curve = $generate.Invoke($generator, @($start, 1.0, $forward, $request))
|
||||
Assert-True ($curve -ne $null) 'Arc primitive must be generated.'
|
||||
Assert-Near ([Math]::Sin(0.50) + 1.0) $curve.End.X 'Arc X must use analytic integration.'
|
||||
Assert-Near (1.0 - [Math]::Cos(0.50) + 1.0) $curve.End.Y 'Arc Y must use analytic integration.'
|
||||
Assert-Near 0.50 $curve.End.Heading 'Arc heading must use signed distance.'
|
||||
|
||||
$reversePrimitive = $generate.Invoke($generator, @($start, 0.0, $reverse, $request))
|
||||
Assert-True ($reversePrimitive -ne $null) 'Reverse primitive must be generated.'
|
||||
Assert-Near 0.50 $reversePrimitive.End.X 'Reverse straight primitive must move behind the vehicle.'
|
||||
|
||||
$request.Goal = [Activator]::CreateInstance($poseType, @(1.30, 1.0, 0.0))
|
||||
$truncated = $generate.Invoke($generator, @($start, 0.0, $forward, $request))
|
||||
Assert-True ($truncated -ne $null) 'Goal-truncated primitive must be generated.'
|
||||
Assert-Near 0.30 $truncated.ActualLengthMeters 'A 0.30m goal must truncate at its first internal point.'
|
||||
Assert-True $truncated.IsGoalTruncation 'Truncated primitive must record its goal source.'
|
||||
Assert-Near 1.30 $truncated.End.X 'Truncated primitive must end at the goal point.'
|
||||
|
||||
$request.Goal = $start
|
||||
$zeroLength = $generate.Invoke($generator, @($start, 0.0, $forward, $request))
|
||||
Assert-True ($zeroLength -ne $null) 'Start-at-goal must produce a zero-length candidate.'
|
||||
Assert-Near 0.0 $zeroLength.ActualLengthMeters 'Start-at-goal candidate must have zero length.'
|
||||
Assert-True $zeroLength.IsGoalTruncation 'Start-at-goal candidate must have goal truncation source.'
|
||||
|
||||
$wrapPose = [Activator]::CreateInstance($poseType, @(1.0, 1.0, (-[Math]::PI + 0.0005)))
|
||||
$wrapGoal = [Activator]::CreateInstance($poseType, @(1.0, 1.0, ([Math]::PI - 0.0005)))
|
||||
$config.GoalHeadingToleranceRadians = 0.002
|
||||
Assert-True $isSatisfied.Invoke($null, @($wrapPose, $wrapGoal, $config, $forward, $forwardOnly)) 'Goal heading tolerance must wrap across pi.'
|
||||
Assert-False $isSatisfied.Invoke($null, @($wrapPose, $wrapGoal, $config, $reverse, $forwardOnly)) 'Goal direction constraint must reject reverse entry.'
|
||||
|
||||
$levelsMethod = Find-Method $generatorType 'GetCurvatureLevels' @($vehicleType, $configurationType)
|
||||
Assert-True ($levelsMethod -ne $null) 'MotionPrimitiveGenerator must expose curvature levels.'
|
||||
$levels = $levelsMethod.Invoke($generator, @($vehicle, $config))
|
||||
Assert-True ($levels.Count -eq 5) 'Five configured curvature levels must be generated.'
|
||||
Assert-Near -1.0 $levels[0] 'First curvature level must be negative maximum.'
|
||||
Assert-Near 0.0 $levels[2] 'Middle curvature level must be straight.'
|
||||
Assert-Near 1.0 $levels[4] 'Last curvature level must be positive maximum.'
|
||||
$adjacent = Find-Method $generatorType 'AreCurvatureLevelsAdjacent' @([int], [int])
|
||||
Assert-True ($adjacent -ne $null) 'MotionPrimitiveGenerator must expose curvature adjacency.'
|
||||
Assert-True $adjacent.Invoke($null, @(2, 3)) 'Neighboring curvature levels must be allowed.'
|
||||
Assert-False $adjacent.Invoke($null, @(2, 4)) 'Non-neighboring curvature levels must be rejected.'
|
||||
|
||||
$heapGenericType = $assembly.GetType($search + 'BinaryMinHeap`1', $true)
|
||||
$heapType = $heapGenericType.MakeGenericType([string])
|
||||
$heap = [Activator]::CreateInstance($heapType)
|
||||
$push = Find-Method $heapType 'Push' @([string], [double], [double], [double])
|
||||
$pop = Find-Method $heapType 'Pop' @()
|
||||
Assert-True ($push -ne $null) 'BinaryMinHeap must expose Push(item, f, h, g).'
|
||||
Assert-True ($pop -ne $null) 'BinaryMinHeap must expose Pop().'
|
||||
$push.Invoke($heap, @('node-a', 8.0, 2.0, 1.0))
|
||||
$push.Invoke($heap, @('node-b', 8.0, 1.0, 1.0))
|
||||
$push.Invoke($heap, @('node-c', 8.0, 1.0, 3.0))
|
||||
$push.Invoke($heap, @('node-d', 7.0, 9.0, 0.0))
|
||||
$push.Invoke($heap, @('node-e', 8.0, 1.0, 3.0))
|
||||
Assert-Equal 'node-d' $pop.Invoke($heap, @()) 'Lower F must win.'
|
||||
Assert-Equal 'node-c' $pop.Invoke($heap, @()) 'Equal F and H must prefer larger G.'
|
||||
Assert-Equal 'node-e' $pop.Invoke($heap, @()) 'Equal F, H and G must preserve insertion order.'
|
||||
Assert-Equal 'node-b' $pop.Invoke($heap, @()) 'Equal F must prefer lower H.'
|
||||
Assert-Equal 'node-a' $pop.Invoke($heap, @()) 'Remaining item must be returned last.'
|
||||
$firstDeterministicHeap = [Activator]::CreateInstance($heapType)
|
||||
$secondDeterministicHeap = [Activator]::CreateInstance($heapType)
|
||||
foreach ($deterministicHeap in @($firstDeterministicHeap, $secondDeterministicHeap)) {
|
||||
$push.Invoke($deterministicHeap, @('repeat-a', 5.0, 2.0, 1.0))
|
||||
$push.Invoke($deterministicHeap, @('repeat-b', 5.0, 1.0, 1.0))
|
||||
$push.Invoke($deterministicHeap, @('repeat-c', 5.0, 1.0, 3.0))
|
||||
}
|
||||
$firstDeterministicOrder = @($pop.Invoke($firstDeterministicHeap, @()), $pop.Invoke($firstDeterministicHeap, @()), $pop.Invoke($firstDeterministicHeap, @())) -join ','
|
||||
$secondDeterministicOrder = @($pop.Invoke($secondDeterministicHeap, @()), $pop.Invoke($secondDeterministicHeap, @()), $pop.Invoke($secondDeterministicHeap, @())) -join ','
|
||||
Assert-Equal $firstDeterministicOrder $secondDeterministicOrder 'Equivalent heap inputs must have deterministic pop order.'
|
||||
|
||||
$calculatorType = $assembly.GetType($search + 'SearchCostCalculator', $true)
|
||||
$calculator = [Activator]::CreateInstance($calculatorType)
|
||||
$calculate = Find-Method $calculatorType 'Calculate' @([double], $directionType, [bool], [double], [double], [int], [double], $configurationType)
|
||||
Assert-True ($calculate -ne $null) 'SearchCostCalculator must expose the planned equivalent-meter calculation.'
|
||||
$costConfig = [Activator]::CreateInstance($configurationType)
|
||||
$cost = $calculate.Invoke($calculator, @(2.0, $reverse, $true, 0.5, 1.0, 2, 0.25, $costConfig))
|
||||
Assert-Near 4.55 $cost 'Reverse, gear, curvature and clearance costs must be accumulated.'
|
||||
$negativeWeightConfig = [Activator]::CreateInstance($configurationType)
|
||||
$negativeWeightConfig.CurvatureMagnitudeWeight = -0.01
|
||||
Assert-Throws { $calculate.Invoke($calculator, @(1.0, $forward, $false, 0.0, 1.0, 0, 1.0, $negativeWeightConfig)) } 'Negative cost weights must be rejected.'
|
||||
$nonFiniteWeightConfig = [Activator]::CreateInstance($configurationType)
|
||||
$nonFiniteWeightConfig.ClearanceCostWeight = [double]::NaN
|
||||
Assert-Throws { $calculate.Invoke($calculator, @(1.0, $forward, $false, 0.0, 1.0, 0, 1.0, $nonFiniteWeightConfig)) } 'Non-finite cost weights must be rejected.'
|
||||
|
||||
$dijkstraType = $assembly.GetType($search + 'GridDijkstraHeuristic', $true)
|
||||
$dijkstraConstructor = $dijkstraType.GetConstructor(@($map.GetType(), [int], [int]))
|
||||
Assert-True ($dijkstraConstructor -ne $null) 'GridDijkstraHeuristic must expose the planned grid constructor.'
|
||||
$getCost = Find-Method $dijkstraType 'GetCost' @([int], [int])
|
||||
Assert-True ($getCost -ne $null) 'GridDijkstraHeuristic must expose GetCost(row, col).'
|
||||
$openHeuristic = $dijkstraConstructor.Invoke(@($map, 2, 2))
|
||||
$expectedDiagonalCost = 2.0 * [Math]::Sqrt(2.0) * $map.ResolutionMeters
|
||||
Assert-Near $expectedDiagonalCost $getCost.Invoke($openHeuristic, @(0, 0)) 'Open-grid diagonal movement must cost sqrt(2) per cell.'
|
||||
$cornerBlockedMap = New-CornerBlockedMap
|
||||
$blockedHeuristic = $dijkstraConstructor.Invoke(@($cornerBlockedMap, 1, 1))
|
||||
Assert-True ([double]::IsPositiveInfinity($getCost.Invoke($blockedHeuristic, @(0, 0)))) 'Diagonal corner cutting through two occupied orthogonal cells must be forbidden.'
|
||||
|
||||
$operationBudgetType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Utils.PlanningOperationBudget', $true)
|
||||
$operationStopReasonType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Utils.PlanningOperationStopReason', $true)
|
||||
$budgetConstructor = $operationBudgetType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null,
|
||||
@([Threading.CancellationToken], [TimeSpan]), $null)
|
||||
Assert-True ($budgetConstructor -ne $null) 'PlanningOperationBudget must expose its internal cancellation and timeout constructor.'
|
||||
$tryCreateDijkstra = Find-NonPublicStaticMethod $dijkstraType 'TryCreate' @($map.GetType(), [int], [int], $operationBudgetType,
|
||||
$dijkstraType.MakeByRefType(), $operationStopReasonType.MakeByRefType())
|
||||
Assert-True ($tryCreateDijkstra -ne $null) 'GridDijkstraHeuristic must expose an internal budget-aware TryCreate method.'
|
||||
$expiredBudget = $budgetConstructor.Invoke(@([Threading.CancellationToken]::None, [TimeSpan]::Zero))
|
||||
$dijkstraArguments = [object[]]@($map, 2, 2, $expiredBudget, $null, $null)
|
||||
$dijkstraCreated = $tryCreateDijkstra.Invoke($null, $dijkstraArguments)
|
||||
Assert-False $dijkstraCreated 'An expired total budget must stop Dijkstra construction.'
|
||||
Assert-Null $dijkstraArguments[4] 'Stopped Dijkstra construction must not publish a partial heuristic.'
|
||||
Assert-Equal 'TimedOut' $dijkstraArguments[5].ToString() 'Expired Dijkstra construction must report timeout.'
|
||||
|
||||
$largeBounds = [Activator]::CreateInstance($assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.MapBoundsMm', $true),
|
||||
@([single]0, [single]40000, [single]0, [single]20000))
|
||||
$largeMapRequest = [Activator]::CreateInstance($assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapRequest', $true))
|
||||
$largeMapRequest.Bounds = $largeBounds
|
||||
$largeMapRequest.ResolutionMm = [single]20
|
||||
$largeMapRequest.AllowExplicitEmptyMap = $true
|
||||
$largeMap = [Activator]::CreateInstance($assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapFactory', $true)).Create($largeMapRequest)
|
||||
Assert-True $largeMap.Succeeded 'Large empty map for Dijkstra cancellation must be created before cancellation starts.'
|
||||
$midDijkstraCancellation = New-Object Threading.CancellationTokenSource
|
||||
$midDijkstraBudget = $budgetConstructor.Invoke(@($midDijkstraCancellation.Token, [TimeSpan]::FromSeconds(2)))
|
||||
$midDijkstraCancellation.CancelAfter(1)
|
||||
$midDijkstraTimer = [Diagnostics.Stopwatch]::StartNew()
|
||||
$midDijkstraArguments = [object[]]@($largeMap.Map, 500, 1000, $midDijkstraBudget, $null, $null)
|
||||
$midDijkstraCreated = $tryCreateDijkstra.Invoke($null, $midDijkstraArguments)
|
||||
$midDijkstraTimer.Stop()
|
||||
Assert-False $midDijkstraCreated 'Cancellation during Dijkstra initialization must stop construction.'
|
||||
Assert-Null $midDijkstraArguments[4] 'Mid-Dijkstra cancellation must not publish a partial heuristic.'
|
||||
Assert-Equal 'Cancelled' $midDijkstraArguments[5].ToString() 'Mid-Dijkstra cancellation must retain cancellation status.'
|
||||
Assert-True ($midDijkstraTimer.Elapsed -lt [TimeSpan]::FromSeconds(2)) 'Dijkstra cancellation must return within the configured response bound.'
|
||||
|
||||
Write-Output 'Coarse path search primitive checks passed.'
|
||||
|
||||
$nodeType = $assembly.GetType($search + 'HybridAStarNode', $true)
|
||||
$nodeKeyType = $assembly.GetType($search + 'HybridAStarNodeKey', $true)
|
||||
$searchType = $assembly.GetType($search + 'HybridAStarSearch', $true)
|
||||
$searchResultType = $assembly.GetType($search + 'HybridAStarSearchResult', $true)
|
||||
$searchMethod = Find-Method $searchType 'Search' @($requestType, [Threading.CancellationToken])
|
||||
Assert-True ($searchMethod -ne $null) 'HybridAStarSearch must expose Search(request, cancellationToken).'
|
||||
Assert-True ($searchResultType.GetProperty('Status') -ne $null) 'Search result must expose planning status.'
|
||||
Assert-True ($searchResultType.GetProperty('TerminationReason') -ne $null) 'Search result must expose its original termination reason.'
|
||||
Assert-True ($searchResultType.GetProperty('SuccessNodeIndex') -ne $null) 'Search result must expose the successful node index.'
|
||||
Assert-True ($searchResultType.GetProperty('ReopenedNodeCount') -ne $null) 'Search result must expose reopened node count.'
|
||||
Assert-True ($searchResultType.GetProperty('StaleOpenListEntryCount') -ne $null) 'Search result must expose stale Open List entry count.'
|
||||
Assert-True ($searchResultType.GetProperty('PeakOpenListCount') -ne $null) 'Search result must expose Open List peak count.'
|
||||
Assert-True ($nodeType.GetProperty('ParentNodeIndex') -ne $null) 'Search node must expose its parent node index.'
|
||||
Assert-True ($nodeKeyType.GetProperty('HeadingIndex') -ne $null) 'Search node key must contain a heading index.'
|
||||
Assert-True ($nodeKeyType.GetProperty('Direction') -ne $null) 'Search node key must contain a travel direction.'
|
||||
Assert-True ($nodeKeyType.GetProperty('CurvatureLevelIndex') -ne $null) 'Search node key must contain a curvature level index.'
|
||||
|
||||
$goalCandidateProperty = $nodeType.GetProperty('IsGoalCandidate')
|
||||
Assert-True ($goalCandidateProperty -ne $null) 'Search node must mark a goal-truncated candidate explicitly.'
|
||||
$nodeConstructor = $nodeType.GetConstructor(@([int], $nodeKeyType, $poseType, [int], $primitiveType, [double], [double], [double], [bool]))
|
||||
Assert-True ($nodeConstructor -ne $null) 'Search node must accept the explicit goal-candidate marker.'
|
||||
$admissionMethod = $searchType.GetMethod('ShouldEnqueueSuccessor', [Reflection.BindingFlags]'NonPublic,Static')
|
||||
Assert-True ($admissionMethod -ne $null) 'Search must expose its private successor admission policy for reflection regression coverage.'
|
||||
|
||||
$candidateStart = [Activator]::CreateInstance($poseType, @(1.000, 1.000, 0.0))
|
||||
$candidateGoal = [Activator]::CreateInstance($poseType, @(1.024, 1.000, 0.0))
|
||||
$candidatePoints = [Array]::CreateInstance($poseType, 1)
|
||||
$candidatePoints.SetValue($candidateGoal, 0)
|
||||
$candidatePrimitive = [Activator]::CreateInstance($primitiveType, @($candidateStart, $forward, 0.0, 0.024, $candidatePoints, [double[]]@(1.0), $true))
|
||||
$sharedKey = [Activator]::CreateInstance($nodeKeyType, @(20, 20, 0, $forward, 2))
|
||||
$normalNode = $nodeConstructor.Invoke(@(10, $sharedKey, $candidateStart, 0, $null, 0.0, 0.250, 0.0, $false))
|
||||
$goalCandidateNode = $nodeConstructor.Invoke(@(11, $sharedKey, $candidateGoal, 10, $candidatePrimitive, 0.0, 0.300, 0.0, $true))
|
||||
$dominanceConfiguration = [Activator]::CreateInstance($configurationType)
|
||||
$dominanceConfiguration.GoalPositionToleranceMeters = 0.001
|
||||
$dominanceConfiguration.GoalHeadingToleranceRadians = 0.001
|
||||
Assert-True $candidatePrimitive.IsGoalTruncation 'Regression setup must use a goal-truncated motion primitive.'
|
||||
Assert-True $normalNode.Key.Equals($goalCandidateNode.Key) 'Regression setup must share one discrete state key.'
|
||||
Assert-False $isSatisfied.Invoke($null, @($normalNode.Pose, $candidateGoal, $dominanceConfiguration, $forward, $forwardOnly)) 'Lower-G normal node must remain outside the continuous goal tolerance.'
|
||||
Assert-True $isSatisfied.Invoke($null, @($goalCandidateNode.Pose, $candidateGoal, $dominanceConfiguration, $forward, $forwardOnly)) 'Goal candidate must satisfy the continuous goal tolerance.'
|
||||
Assert-False $admissionMethod.Invoke($null, @($normalNode, 0.250)) 'Equal-cost normal state must be dominated by the best same-key label.'
|
||||
Assert-True $admissionMethod.Invoke($null, @($goalCandidateNode, 0.250)) 'Goal candidate must enter Open List even when a lower-G normal state has the same key.'
|
||||
|
||||
function New-SearchRequest([object]$Goal, [object]$GoalDirection, [bool]$AllowReverse) {
|
||||
$searchRequest = [Activator]::CreateInstance($requestType)
|
||||
$searchRequest.Map = $map
|
||||
$searchRequest.Start = [Activator]::CreateInstance($poseType, @(1.0, 1.0, 0.0))
|
||||
$searchRequest.Goal = $Goal
|
||||
$searchRequest.Vehicle = $vehicle
|
||||
$searchConfiguration = [Activator]::CreateInstance($configurationType)
|
||||
$searchConfiguration.PrimitiveLengthMeters = 0.50
|
||||
$searchConfiguration.IntegrationStepMeters = 0.05
|
||||
$searchConfiguration.MaximumCollisionCheckStepMeters = 0.025
|
||||
$searchConfiguration.GoalPositionToleranceMeters = 0.001
|
||||
$searchConfiguration.GoalHeadingToleranceRadians = 0.001
|
||||
$searchConfiguration.CurvatureLevelCount = 5
|
||||
$searchConfiguration.MaximumExpandedNodes = 10000
|
||||
$searchConfiguration.SearchTimeout = [TimeSpan]::FromSeconds(2.0)
|
||||
$searchConfiguration.AllowReverse = $AllowReverse
|
||||
$searchRequest.Configuration = $searchConfiguration
|
||||
$searchRequest.GoalDirection = $GoalDirection
|
||||
return $searchRequest
|
||||
}
|
||||
|
||||
$searcher = [Activator]::CreateInstance($searchType)
|
||||
$searchGoal = [Activator]::CreateInstance($poseType, @(1.20, 1.0, 0.0))
|
||||
$searchRequest = New-SearchRequest $searchGoal $forwardOnly $false
|
||||
$searchResult = $searchMethod.Invoke($searcher, @($searchRequest, [Threading.CancellationToken]::None))
|
||||
Assert-Equal 'Success' $searchResult.Status.ToString() 'Empty map forward search must succeed.'
|
||||
Assert-True ($null -ne $searchResult.SuccessNodeIndex) 'Success must be reported only with a popped goal candidate node.'
|
||||
Assert-True ($searchResult.ExpandedNodeCount -gt 0) 'A successful search must expand the selected goal candidate.'
|
||||
|
||||
$atGoal = New-SearchRequest $searchRequest.Start $forwardOnly $false
|
||||
$atGoalResult = $searchMethod.Invoke($searcher, @($atGoal, [Threading.CancellationToken]::None))
|
||||
Assert-Equal 'Success' $atGoalResult.Status.ToString() 'A zero-length goal candidate must enter and leave the open list successfully.'
|
||||
Assert-True ($null -ne $atGoalResult.SuccessNodeIndex) 'A zero-length goal candidate must have a node index.'
|
||||
|
||||
$cancelledRequest = New-SearchRequest $searchGoal $forwardOnly $false
|
||||
$cancellationSource = New-Object Threading.CancellationTokenSource
|
||||
$cancellationSource.Cancel()
|
||||
$cancelledResult = $searchMethod.Invoke($searcher, @($cancelledRequest, $cancellationSource.Token))
|
||||
Assert-Equal 'Cancelled' $cancelledResult.Status.ToString() 'Cancellation must be observed before node expansion.'
|
||||
Assert-True (-not [string]::IsNullOrWhiteSpace($cancelledResult.TerminationReason)) 'Cancelled search must retain a non-empty reason.'
|
||||
|
||||
$limitedRequest = New-SearchRequest $searchGoal $forwardOnly $false
|
||||
$limitedRequest.Configuration.MaximumExpandedNodes = 0
|
||||
$limitedResult = $searchMethod.Invoke($searcher, @($limitedRequest, [Threading.CancellationToken]::None))
|
||||
Assert-Equal 'SearchNodeLimitExceeded' $limitedResult.Status.ToString() 'Node limit must be checked before expansion.'
|
||||
Assert-True (-not [string]::IsNullOrWhiteSpace($limitedResult.TerminationReason)) 'Node-limited search must retain a non-empty reason.'
|
||||
|
||||
$timedOutRequest = New-SearchRequest $searchGoal $forwardOnly $false
|
||||
$timedOutRequest.Configuration.SearchTimeout = [TimeSpan]::Zero
|
||||
$timedOutResult = $searchMethod.Invoke($searcher, @($timedOutRequest, [Threading.CancellationToken]::None))
|
||||
Assert-Equal 'SearchTimeout' $timedOutResult.Status.ToString() 'Timeout must be checked before expansion.'
|
||||
Assert-True (-not [string]::IsNullOrWhiteSpace($timedOutResult.TerminationReason)) 'Timed-out search must retain a non-empty reason.'
|
||||
Assert-False ($cancelledResult.TerminationReason -eq $limitedResult.TerminationReason) 'Cancelled and node-limited searches must retain different reasons.'
|
||||
Assert-False ($limitedResult.TerminationReason -eq $timedOutResult.TerminationReason) 'Node-limited and timed-out searches must retain different reasons.'
|
||||
|
||||
$internalSearchMethod = $searchType.GetMethods([Reflection.BindingFlags]'Instance,NonPublic') |
|
||||
Where-Object {
|
||||
$_.Name -eq 'Search' -and
|
||||
$_.GetParameters().Length -eq 2 -and
|
||||
$_.GetParameters()[1].ParameterType -eq $operationBudgetType
|
||||
} |
|
||||
Select-Object -First 1
|
||||
Assert-True ($internalSearchMethod -ne $null) 'Search must retain its internal shared-budget overload.'
|
||||
$internalErrorResult = $internalSearchMethod.Invoke($searcher, @($searchRequest, $null))
|
||||
Assert-Equal 'InternalError' $internalErrorResult.Status.ToString() 'A missing internal budget must be mapped to InternalError.'
|
||||
Assert-True ($internalErrorResult.TerminationReason.Contains('ArgumentNullException')) 'Internal search errors must retain the exception type.'
|
||||
|
||||
Write-Output 'Coarse path Hybrid A star search checks passed.'
|
||||
@@ -0,0 +1,122 @@
|
||||
$ErrorActionPreference = 'Stop'
|
||||
|
||||
function Assert-True([bool]$condition, [string]$message) {
|
||||
if (-not $condition) { throw $message }
|
||||
}
|
||||
|
||||
function Assert-Match([string]$content, [string]$pattern, [string]$message) {
|
||||
Assert-True ($content -match $pattern) $message
|
||||
}
|
||||
|
||||
function Assert-NotMatch([string]$content, [string]$pattern, [string]$message) {
|
||||
Assert-True ($content -notmatch $pattern) $message
|
||||
}
|
||||
|
||||
$sourcePath = Join-Path $PSScriptRoot '..\ParkrobTrajplanner\CoarsePath\Test\MovementTest.CoarsePathTest.cs'
|
||||
Assert-True (Test-Path -LiteralPath $sourcePath) 'P1 coarse-path UI movement-test source must exist.'
|
||||
$source = Get-Content -Raw -Encoding UTF8 $sourcePath
|
||||
|
||||
Assert-Match $source 'class\s+CoarsePathPlanningTest\s*:\s*MovementTest' 'The UI entry must inherit MovementTest.'
|
||||
Assert-Match $source 'namespace\s+MultiWheelC\s*;' 'MovementTest entries must use the project discovery namespace.'
|
||||
Assert-Match $source 'CoarsePathPlanningService' 'The UI must keep one shared planning service for map-cache reuse.'
|
||||
Assert-Match $source 'Task\.Run\s*\(' 'Planning must run in a background Task.'
|
||||
Assert-Match $source 'CancellationTokenSource' 'The UI must retain a cancellation source for its active task.'
|
||||
Assert-Match $source 'override\s+void\s+TestStop\s*\(' 'The UI must implement TestStop cancellation.'
|
||||
Assert-Match $source 'CoarsePathScenarioFactory\.Create\s*\(' 'The UI must expose fixed scenario entries through the factory.'
|
||||
Assert-Match $source 'ManualCoarsePathObstacle' 'The manual UI must construct typed manual obstacles.'
|
||||
Assert-Match $source 'CreateManualObstacleDemo\s*\(' 'The manual UI must submit obstacles through the factory.'
|
||||
Assert-Match $source 'MaximumManualObstacleCount\s*=\s*20' 'The manual UI must bound obstacle input to 20.'
|
||||
Assert-Match $source 'ReadManualObstacles\s*\(' 'The manual UI must read the requested obstacle sequence.'
|
||||
Assert-Match $source 'Interlocked\.Increment\s*\(' 'The manual UI must issue a fresh obstacle snapshot version.'
|
||||
Assert-Match $source 'Circle\s*\(' 'The manual UI must support circle input.'
|
||||
Assert-Match $source 'AxisAlignedRectangle\s*\(' 'The manual UI must support axis-aligned rectangle input.'
|
||||
Assert-Match $source 'UI\.GetInput\s*\(' 'The manual demo must accept user input.'
|
||||
Assert-Match $source 'ReadPositiveTimeoutInput\s*\(' 'The manual UI must read a finite positive timeout.'
|
||||
Assert-Match $source 'Configuration\.SearchTimeout\s*=\s*searchTimeout' 'The manual UI must apply the timeout to the current job.'
|
||||
Assert-Match $source 'TimeSpan\.FromSeconds\s*\(' 'The manual timeout must convert seconds to TimeSpan.'
|
||||
Assert-Match $source 'timeoutSeconds\s*<=\s*0' 'The manual timeout must reject zero and negative values.'
|
||||
Assert-Match $source 'getCartLocation\s*\(' 'The manual demo must read the AMR body-center pose.'
|
||||
Assert-Match $source 'DrawLegend\s*\(' 'The painter output must include a visual legend.'
|
||||
Assert-Match $source 'PlanningGridMap' 'The painter output must consume the planning grid result.'
|
||||
Assert-Match $source '\.IsOccupied\s*\(' 'The painter output must draw occupied grid cells.'
|
||||
Assert-Match $source 'ResolutionMm' 'The painter output must draw the grid at its true resolution.'
|
||||
Assert-Match $source 'SnapshotId' 'The painter output must show the map snapshot identity.'
|
||||
Assert-Match $source 'IsGearSwitchPoint' 'The painter output must highlight gear-switch points.'
|
||||
Assert-Match $source 'GoalPositionToleranceMeters' 'The painter output must draw goal tolerance.'
|
||||
Assert-True (([regex]::Matches($source, 'PathSearchElapsed')).Count -ge 2) 'The status layer and Toast must both show path-search elapsed time.'
|
||||
Assert-Match $source 'BuildToastMessage[\s\S]*TerminationReason' 'The failure Toast must include the termination reason.'
|
||||
Assert-Match $source 'ExpandedNodeCount' 'The status layer must show expanded-node statistics.'
|
||||
Assert-Match $source 'VehicleKinematics\.TryGetMaximumCurvaturePerMeter' 'The status layer must show the effective turning radius.'
|
||||
Assert-NotMatch $source 'PlanningMapFactory' 'Movement tests must not build maps directly; use CoarsePathPlanningService.'
|
||||
Assert-NotMatch $source '\.Result\b' 'The UI must not block on Task.Result.'
|
||||
Assert-NotMatch $source '\.Wait\s*\(' 'The UI must not block the movement-test thread.'
|
||||
|
||||
$runnerStart = $source.IndexOf('internal static class CoarsePathPlanningTestRunner')
|
||||
Assert-True ($runnerStart -ge 0) 'Shared runner source must exist.'
|
||||
$runnerSource = $source.Substring($runnerStart)
|
||||
Assert-Match $runnerSource 'RunScenario[\s\S]*getCartLocation\s*\(' 'Fixed scenarios must read the current AMR pose.'
|
||||
Assert-Match $runnerSource 'CoarsePathScenarioFactory\.Create\s*\(\s*scenario\s*,' 'Fixed scenarios must use the AMR-aware factory overload.'
|
||||
Assert-Match $runnerSource 'AMR' 'The runner must expose AMR pose diagnostics.'
|
||||
Assert-Match $runnerSource 'ArgumentException\("AMR' 'Invalid AMR input must be reported without starting planning.'
|
||||
Assert-Match $runnerSource 'double\.IsNaN|double\.IsInfinity' 'The runner must reject non-finite AMR coordinates.'
|
||||
Assert-Match $runnerSource 'DrawStatus\s*\([\s\S]*AmrPoseSnapshot' 'Result status must receive the frozen AMR snapshot.'
|
||||
$inputFailureStart = $runnerSource.IndexOf('internal static void ShowInputFailure')
|
||||
$inputFailureEnd = $runnerSource.IndexOf('private static void Finish', $inputFailureStart)
|
||||
Assert-True ($inputFailureStart -ge 0 -and $inputFailureEnd -gt $inputFailureStart) 'Input failure renderer must be isolated before completion handling.'
|
||||
$inputFailureSource = $runnerSource.Substring($inputFailureStart, $inputFailureEnd - $inputFailureStart)
|
||||
Assert-Match $inputFailureSource 'Painter\.DrawText\s*\(' 'Invalid AMR input must be shown on the Painter status layer.'
|
||||
|
||||
$readmePath = Join-Path $PSScriptRoot '..\ParkrobTrajplanner\CoarsePath\README.md'
|
||||
Assert-True (Test-Path -LiteralPath $readmePath) 'CoarsePath README must exist beside its module.'
|
||||
$readme = Get-Content -Raw -Encoding UTF8 $readmePath
|
||||
foreach ($requiredText in @(
|
||||
'CoarsePathPlanningV1',
|
||||
'CoarsePathPlanningTest',
|
||||
'CreateManualGoalDemo',
|
||||
'getCartLocation',
|
||||
'mm',
|
||||
'deg',
|
||||
'rad',
|
||||
'CancellationTokenSource',
|
||||
'TestStop',
|
||||
'NoFeasiblePath',
|
||||
'IsGearSwitchPoint')) {
|
||||
Assert-True ($readme.Contains($requiredText)) "P1 README must document $requiredText."
|
||||
}
|
||||
|
||||
$readmeStructure = @(
|
||||
'File Structure',
|
||||
'Planning Data Flow',
|
||||
'Build Status and Stop',
|
||||
'Coordinates and Units',
|
||||
'Minimal Call Example',
|
||||
'Cache and SourceVersion',
|
||||
'Detailed Usage Guide',
|
||||
'P1 Manual Tests and Visualization',
|
||||
'Common Errors',
|
||||
'First-Version Limits',
|
||||
'CoarsePathPlanningService.Plan(job, cancellationToken)',
|
||||
'CoarsePathPlanningJob',
|
||||
'PlanningGridMap',
|
||||
'SourceVersion',
|
||||
'CoarsePathPlanningV1',
|
||||
'CancellationTokenSource',
|
||||
'NoFeasiblePath',
|
||||
'IsGearSwitchPoint',
|
||||
'CreateManualObstacleDemo',
|
||||
'ManualCoarsePathObstacle',
|
||||
'manual-user-input',
|
||||
'0-20',
|
||||
'PathSearchElapsed',
|
||||
'1.20 m',
|
||||
'Reeds-Shepp',
|
||||
'AxisAlignedRectangle',
|
||||
'Create(CoarsePathTestScenario scenario, double amrXMillimeters',
|
||||
'DetectionHeadingRadians',
|
||||
'../Map/README.md'
|
||||
)
|
||||
foreach ($requiredText in $readmeStructure) {
|
||||
Assert-True ($readme.Contains($requiredText)) "Restructured CoarsePath README must document $requiredText."
|
||||
}
|
||||
|
||||
Write-Output 'Coarse path P1 UI source checks passed.'
|
||||
@@ -0,0 +1,121 @@
|
||||
param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
|
||||
|
||||
$ErrorActionPreference = 'Stop'
|
||||
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
|
||||
|
||||
function Assert-True($Actual, [string]$Message) {
|
||||
if (-not $Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-Near([double]$Expected, [double]$Actual, [string]$Message) {
|
||||
if ([Math]::Abs($Expected - $Actual) -gt 0.000000001) {
|
||||
throw "$Message Expected=$Expected Actual=$Actual"
|
||||
}
|
||||
}
|
||||
|
||||
function Assert-Throws([scriptblock]$Action, [string]$Message) {
|
||||
try {
|
||||
& $Action
|
||||
}
|
||||
catch {
|
||||
return
|
||||
}
|
||||
|
||||
throw $Message
|
||||
}
|
||||
|
||||
function Get-RequiredType([string]$Name) {
|
||||
return $assembly.GetType($Name, $true)
|
||||
}
|
||||
|
||||
function Get-RequiredProperty($Type, [string]$Name) {
|
||||
$property = $Type.GetProperty($Name, [Reflection.BindingFlags]'Instance,Public,NonPublic')
|
||||
Assert-True ($null -ne $property) ("Missing property: " + $Name)
|
||||
Assert-True (-not $property.CanWrite) ("Snapshot property must be get-only: " + $Name)
|
||||
return $property
|
||||
}
|
||||
|
||||
$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
|
||||
$algorithms = $root + 'Algorithms.'
|
||||
$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
|
||||
$mapping = 'MultiWheelC.TrajectoryPlanning.Mapping.'
|
||||
|
||||
$snapshotType = Get-RequiredType ($algorithms + 'SmoothingOptionsSnapshot')
|
||||
$inputType = Get-RequiredType ($algorithms + 'SmoothingAlgorithmInput')
|
||||
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
|
||||
$requestType = Get-RequiredType ($root + 'PathSmoothingRequest')
|
||||
$preparedPathType = Get-RequiredType ($root + 'Processing.PreparedPath')
|
||||
$mapType = Get-RequiredType ($mapping + 'PlanningGridMap')
|
||||
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
|
||||
$coarsePointType = Get-RequiredType ($coarsePath + 'CoarsePathPoint')
|
||||
$pathSegmentType = Get-RequiredType ($coarsePath + 'PathSegment')
|
||||
|
||||
$snapshotConstructor = $snapshotType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null,
|
||||
@($configurationType), $null)
|
||||
Assert-True ($null -ne $snapshotConstructor) 'SmoothingOptionsSnapshot must be created from PathSmoothingConfiguration.'
|
||||
|
||||
$inputConstructor = $inputType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null,
|
||||
@($preparedPathType, $mapType, $vehicleType, [double], [double], $snapshotType), $null)
|
||||
Assert-True ($null -ne $inputConstructor) 'SmoothingAlgorithmInput must accept the immutable smoothing-options snapshot at its construction boundary.'
|
||||
|
||||
$optionNames = @(
|
||||
'CubicBSplineEndpointTangentScale',
|
||||
'BezierCornerHeadingThresholdRadians',
|
||||
'BezierMaximumWindowLengthMeters',
|
||||
'BezierHandleLengthRatio',
|
||||
'QuinticKnotSpacingMeters',
|
||||
'QuinticMinimumKnotSpacingMeters')
|
||||
$optionProperties = @{}
|
||||
foreach ($optionName in $optionNames) {
|
||||
$optionProperties[$optionName] = Get-RequiredProperty $snapshotType $optionName
|
||||
}
|
||||
|
||||
function New-Configuration {
|
||||
return [Activator]::CreateInstance($configurationType)
|
||||
}
|
||||
|
||||
function New-Snapshot($Configuration) {
|
||||
return $snapshotConstructor.Invoke(@($Configuration))
|
||||
}
|
||||
|
||||
# Request creation takes a configuration copy. Later mutations to either the source configuration
|
||||
# or a configuration copy returned by the request must not alter the algorithm snapshot.
|
||||
$configuration = New-Configuration
|
||||
$configuration.CubicBSpline.EndpointTangentScale = [double]0.20
|
||||
$configuration.LocalCubicBezier.CornerHeadingThresholdRadians = [double]0.40
|
||||
$configuration.LocalCubicBezier.MaximumWindowLengthMeters = [double]0.80
|
||||
$configuration.LocalCubicBezier.HandleLengthRatio = [double]0.25
|
||||
$configuration.PiecewiseQuintic.KnotSpacingMeters = [double]0.60
|
||||
$configuration.PiecewiseQuintic.MinimumKnotSpacingMeters = [double]0.15
|
||||
$emptyCoarsePath = [Array]::CreateInstance($coarsePointType, 0)
|
||||
$emptySegments = [Array]::CreateInstance($pathSegmentType, 0)
|
||||
$request = [Activator]::CreateInstance($requestType, @($emptyCoarsePath, $emptySegments, $null, $null, $configuration))
|
||||
$configuration.CubicBSpline.EndpointTangentScale = [double]0.90
|
||||
$requestConfiguration = $request.Configuration
|
||||
Assert-Near 0.20 $requestConfiguration.CubicBSpline.EndpointTangentScale 'Request configuration must remain independent from source-config mutations.'
|
||||
$snapshot = New-Snapshot $requestConfiguration
|
||||
$requestConfiguration.CubicBSpline.EndpointTangentScale = [double]0.70
|
||||
Assert-Near 0.20 $optionProperties['CubicBSplineEndpointTangentScale'].GetValue($snapshot) 'Algorithm options must remain independent from request-configuration mutations.'
|
||||
Assert-Near 0.40 $optionProperties['BezierCornerHeadingThresholdRadians'].GetValue($snapshot) 'Snapshot must preserve the request configuration values.'
|
||||
Assert-Near 0.80 $optionProperties['BezierMaximumWindowLengthMeters'].GetValue($snapshot) 'Snapshot must preserve the request configuration values.'
|
||||
Assert-Near 0.25 $optionProperties['BezierHandleLengthRatio'].GetValue($snapshot) 'Snapshot must preserve the request configuration values.'
|
||||
Assert-Near 0.60 $optionProperties['QuinticKnotSpacingMeters'].GetValue($snapshot) 'Snapshot must preserve the request configuration values.'
|
||||
Assert-Near 0.15 $optionProperties['QuinticMinimumKnotSpacingMeters'].GetValue($snapshot) 'Snapshot must preserve the request configuration values.'
|
||||
|
||||
function Assert-InvalidSnapshot([scriptblock]$Mutate, [string]$Message) {
|
||||
$invalidConfiguration = New-Configuration
|
||||
& $Mutate $invalidConfiguration
|
||||
Assert-Throws { New-Snapshot $invalidConfiguration } $Message
|
||||
}
|
||||
|
||||
Assert-InvalidSnapshot { param($c) $c.CubicBSpline.EndpointTangentScale = [double]::NaN } 'Non-finite B-spline tangent scale must be rejected before retry.'
|
||||
Assert-InvalidSnapshot { param($c) $c.CubicBSpline.EndpointTangentScale = [double]0.0 } 'Non-positive B-spline tangent scale must be rejected before retry.'
|
||||
Assert-InvalidSnapshot { param($c) $c.LocalCubicBezier.CornerHeadingThresholdRadians = [double]0.0 } 'A zero Bézier heading threshold must be rejected before retry.'
|
||||
Assert-InvalidSnapshot { param($c) $c.LocalCubicBezier.CornerHeadingThresholdRadians = [Math]::PI + 0.0001 } 'A Bézier heading threshold above pi must be rejected before retry.'
|
||||
Assert-InvalidSnapshot { param($c) $c.LocalCubicBezier.MaximumWindowLengthMeters = [double]::PositiveInfinity } 'A non-finite Bézier window length must be rejected before retry.'
|
||||
Assert-InvalidSnapshot { param($c) $c.LocalCubicBezier.HandleLengthRatio = [double]0.0 } 'A non-positive Bézier handle ratio must be rejected before retry.'
|
||||
Assert-InvalidSnapshot { param($c) $c.PiecewiseQuintic.KnotSpacingMeters = [double]0.0 } 'A non-positive quintic knot spacing must be rejected before retry.'
|
||||
Assert-InvalidSnapshot { param($c) $c.PiecewiseQuintic.MinimumKnotSpacingMeters = [double]::NaN } 'A non-finite quintic minimum knot spacing must be rejected before retry.'
|
||||
Assert-InvalidSnapshot { param($c) $c.PiecewiseQuintic.KnotSpacingMeters = [double]0.05; $c.PiecewiseQuintic.MinimumKnotSpacingMeters = [double]0.10 } 'Quintic knot spacing below the configured minimum must be rejected before retry.'
|
||||
|
||||
Write-Output 'Path smoothing algorithm-input checks passed.'
|
||||
@@ -0,0 +1,319 @@
|
||||
param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
|
||||
|
||||
$ErrorActionPreference = 'Stop'
|
||||
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
|
||||
|
||||
function Assert-True($Actual, [string]$Message) {
|
||||
if (-not $Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-Equal($Expected, $Actual, [string]$Message) {
|
||||
if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" }
|
||||
}
|
||||
|
||||
function Assert-Near([double]$Expected, [double]$Actual, [double]$Tolerance, [string]$Message) {
|
||||
if ([Math]::Abs($Expected - $Actual) -gt $Tolerance) {
|
||||
throw "$Message Expected=$Expected Actual=$Actual Tolerance=$Tolerance"
|
||||
}
|
||||
}
|
||||
|
||||
function Get-RequiredType([string]$Name) {
|
||||
return $assembly.GetType($Name, $true)
|
||||
}
|
||||
|
||||
function Get-PropertyValue($Instance, [string]$Name) {
|
||||
$property = $Instance.GetType().GetProperty($Name, [Reflection.BindingFlags]'Instance,Public,NonPublic')
|
||||
Assert-True ($null -ne $property) ("Missing property: " + $Name)
|
||||
return $property.GetValue($Instance)
|
||||
}
|
||||
|
||||
function Assert-PointBitwiseEqual($Expected, $Actual, [string]$Message) {
|
||||
foreach ($name in @('X', 'Y', 'ArcLength', 'Heading', 'UnwrappedHeading', 'BodyClearance')) {
|
||||
$expectedBits = [BitConverter]::DoubleToInt64Bits([double]$Expected.$name)
|
||||
$actualBits = [BitConverter]::DoubleToInt64Bits([double]$Actual.$name)
|
||||
Assert-Equal $expectedBits $actualBits ($Message + ' ' + $name)
|
||||
}
|
||||
Assert-Equal $Expected.IsGearSwitchPoint $Actual.IsGearSwitchPoint ($Message + ' IsGearSwitchPoint')
|
||||
Assert-Equal $Expected.Source.ToString() $Actual.Source.ToString() ($Message + ' Source')
|
||||
}
|
||||
|
||||
function New-Point(
|
||||
[double]$X,
|
||||
[double]$Y,
|
||||
[double]$ArcLength,
|
||||
[double]$Heading,
|
||||
[double]$BodyClearance = 0.10,
|
||||
[bool]$IsGearSwitch = $false) {
|
||||
return [Activator]::CreateInstance($pointType, @(
|
||||
$X, $Y, $ArcLength, $Heading, $Heading, $BodyClearance, $IsGearSwitch, $anchor))
|
||||
}
|
||||
|
||||
function New-DirectionSegment(
|
||||
[int]$Index,
|
||||
$Direction,
|
||||
[object[]]$Points,
|
||||
[bool]$StartsAtGearSwitch = $false,
|
||||
[bool]$EndsAtGearSwitch = $false) {
|
||||
$typedPoints = [Array]::CreateInstance($pointType, $Points.Count)
|
||||
for ($pointIndex = 0; $pointIndex -lt $Points.Count; $pointIndex++) {
|
||||
$typedPoints.SetValue($Points[$pointIndex], $pointIndex)
|
||||
}
|
||||
return [Activator]::CreateInstance($segmentType, @(
|
||||
$Index, $Direction, $typedPoints, $StartsAtGearSwitch, $EndsAtGearSwitch))
|
||||
}
|
||||
|
||||
function New-EmptyMap {
|
||||
$request = [Activator]::CreateInstance($mapRequestType)
|
||||
$request.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000))
|
||||
$request.ResolutionMm = [single]50
|
||||
$request.AllowExplicitEmptyMap = $true
|
||||
$map = [Activator]::CreateInstance($mapFactoryType).Create($request).Map
|
||||
Assert-True ($null -ne $map) 'Bézier test must create an explicit empty planning map.'
|
||||
return $map
|
||||
}
|
||||
|
||||
function New-AlgorithmInput(
|
||||
[object[]]$Segments,
|
||||
[double]$ReserveMeters = 0.02,
|
||||
[double]$CornerThresholdRadians = ([Math]::PI / 18.0),
|
||||
[double]$MaximumWindowLengthMeters = 0.60,
|
||||
[double]$HandleLengthRatio = (1.0 / 3.0)) {
|
||||
$typedSegments = [Array]::CreateInstance($segmentType, $Segments.Count)
|
||||
for ($index = 0; $index -lt $Segments.Count; $index++) {
|
||||
$typedSegments.SetValue($Segments[$index], $index)
|
||||
}
|
||||
$preparedPath = [Activator]::CreateInstance($preparedPathType, [object[]]@(,$typedSegments))
|
||||
$vehicle = [Activator]::CreateInstance($vehicleType)
|
||||
$vehicle.LengthMeters = [double]0.20
|
||||
$vehicle.WidthMeters = [double]0.20
|
||||
$vehicle.SafetyMarginMeters = [double]0.0
|
||||
$vehicle.MaximumCurvaturePerMeter = [double]100.0
|
||||
$vehicle.MinimumTurningRadiusMeters = [double]0.01
|
||||
$configuration = [Activator]::CreateInstance($configurationType)
|
||||
$configuration.LocalCubicBezier.CornerHeadingThresholdRadians = $CornerThresholdRadians
|
||||
$configuration.LocalCubicBezier.MaximumWindowLengthMeters = $MaximumWindowLengthMeters
|
||||
$configuration.LocalCubicBezier.HandleLengthRatio = $HandleLengthRatio
|
||||
$options = $optionsConstructor.Invoke(@($configuration))
|
||||
return $inputConstructor.Invoke(@($preparedPath, (New-EmptyMap), $vehicle, [double]0.05, $ReserveMeters, $options))
|
||||
}
|
||||
|
||||
function Invoke-Candidate(
|
||||
[object[]]$Segments,
|
||||
[double]$ReserveMeters = 0.02,
|
||||
[double]$CornerThresholdRadians = ([Math]::PI / 18.0),
|
||||
[double]$MaximumWindowLengthMeters = 0.60,
|
||||
[double]$HandleLengthRatio = (1.0 / 3.0),
|
||||
[double]$Strength = 1.0) {
|
||||
return $smoothMethod.Invoke($smoother, @(
|
||||
(New-AlgorithmInput $Segments $ReserveMeters $CornerThresholdRadians $MaximumWindowLengthMeters $HandleLengthRatio),
|
||||
$Strength,
|
||||
[Threading.CancellationToken]::None))
|
||||
}
|
||||
|
||||
function Invoke-Smoothing(
|
||||
[object[]]$Segments,
|
||||
[double]$ReserveMeters = 0.02,
|
||||
[double]$CornerThresholdRadians = ([Math]::PI / 18.0),
|
||||
[double]$MaximumWindowLengthMeters = 0.60,
|
||||
[double]$HandleLengthRatio = (1.0 / 3.0),
|
||||
[double]$Strength = 1.0) {
|
||||
$candidate = Invoke-Candidate $Segments $ReserveMeters $CornerThresholdRadians $MaximumWindowLengthMeters $HandleLengthRatio $Strength
|
||||
Assert-True (Get-PropertyValue $candidate 'Succeeded') 'Bézier smoothing must produce a candidate for the deterministic fixture.'
|
||||
return @(Get-PropertyValue $candidate 'Segments')
|
||||
}
|
||||
|
||||
function Get-InterpolatedRunCount($Points) {
|
||||
$runCount = 0
|
||||
$inRun = $false
|
||||
foreach ($point in $Points) {
|
||||
$interpolated = $point.Source.ToString() -eq 'Interpolated'
|
||||
if ($interpolated -and -not $inRun) { $runCount++ }
|
||||
$inRun = $interpolated
|
||||
}
|
||||
return $runCount
|
||||
}
|
||||
|
||||
function Get-PointAtArcLength($Points, [double]$ArcLength) {
|
||||
foreach ($point in $Points) {
|
||||
if ([BitConverter]::DoubleToInt64Bits([double]$point.ArcLength) -eq
|
||||
[BitConverter]::DoubleToInt64Bits($ArcLength)) {
|
||||
return $point
|
||||
}
|
||||
}
|
||||
throw "No output point found at local arc length $ArcLength."
|
||||
}
|
||||
|
||||
function Get-FirstInterpolatedPoint($Points) {
|
||||
foreach ($point in $Points) {
|
||||
if ($point.Source.ToString() -eq 'Interpolated') { return $point }
|
||||
}
|
||||
throw 'Expected an interpolated Bézier point.'
|
||||
}
|
||||
|
||||
$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
|
||||
$processing = $root + 'Processing.'
|
||||
$algorithms = $root + 'Algorithms.'
|
||||
$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
|
||||
|
||||
$smootherType = Get-RequiredType ($algorithms + 'LocalCubicBezierSmoother')
|
||||
$pointType = Get-RequiredType ($processing + 'SmoothingPoint2D')
|
||||
$segmentType = Get-RequiredType ($processing + 'PreparedDirectionSegment')
|
||||
$preparedPathType = Get-RequiredType ($processing + 'PreparedPath')
|
||||
$inputType = Get-RequiredType ($algorithms + 'SmoothingAlgorithmInput')
|
||||
$optionsType = Get-RequiredType ($algorithms + 'SmoothingOptionsSnapshot')
|
||||
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
|
||||
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
|
||||
$directionType = Get-RequiredType ($coarsePath + 'TravelDirection')
|
||||
$sourceType = Get-RequiredType ($root + 'SmoothedPathPointSource')
|
||||
$candidateStatusType = Get-RequiredType ($algorithms + 'SmoothingCandidateStatus')
|
||||
$boundsType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.MapBoundsMm'
|
||||
$mapType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningGridMap'
|
||||
$mapRequestType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapRequest'
|
||||
$mapFactoryType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapFactory'
|
||||
|
||||
$inputConstructor = $inputType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null,
|
||||
@($preparedPathType, $mapType, $vehicleType, [double], [double], $optionsType), $null)
|
||||
Assert-True ($null -ne $inputConstructor) 'Bézier tests must construct algorithm input with immutable option values.'
|
||||
$optionsConstructor = $optionsType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null, @($configurationType), $null)
|
||||
Assert-True ($null -ne $optionsConstructor) 'Bézier tests must create immutable option snapshots.'
|
||||
$smoother = [Activator]::CreateInstance($smootherType, $true)
|
||||
$smoothMethod = $smootherType.GetMethod('Smooth', [Reflection.BindingFlags]'Instance,Public')
|
||||
Assert-True ($null -ne $smoothMethod) 'LocalCubicBezierSmoother must implement the internal smoother contract.'
|
||||
Assert-Equal 'LocalCubicBezier' $smoother.Method.ToString() 'Bézier smoother must identify its public smoothing method.'
|
||||
|
||||
$forward = [Enum]::Parse($directionType, 'Forward')
|
||||
$reverse = [Enum]::Parse($directionType, 'Reverse')
|
||||
$anchor = [Enum]::Parse($sourceType, 'Anchor')
|
||||
|
||||
# A straight path must not create a local Bézier window or alter any sample.
|
||||
$straightSource = @(
|
||||
(New-Point 0.0 0.0 0.0 0.0),
|
||||
(New-Point 0.1 0.0 0.1 0.0),
|
||||
(New-Point 0.2 0.0 0.2 0.0),
|
||||
(New-Point 0.3 0.0 0.3 0.0))
|
||||
$straightOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $straightSource)))[0].Points
|
||||
Assert-Equal 0 (Get-InterpolatedRunCount $straightOutput) 'A straight line must create no Bézier replacement window.'
|
||||
Assert-Equal $straightSource.Count $straightOutput.Count 'A straight line must retain its original sample count.'
|
||||
for ($index = 0; $index -lt $straightSource.Count; $index++) {
|
||||
Assert-PointBitwiseEqual $straightSource[$index] $straightOutput[$index] 'Straight samples must remain bitwise unchanged.'
|
||||
}
|
||||
|
||||
# One corner is one local replacement: only the corner sample is evaluated while the window endpoints stay fixed.
|
||||
$cornerSource = @(
|
||||
(New-Point 0.0 0.0 0.0 0.0),
|
||||
(New-Point 0.1 0.0 0.1 0.0),
|
||||
(New-Point 0.2 0.0 0.2 0.0),
|
||||
(New-Point 0.2 0.1 0.3 ([Math]::PI / 2.0)),
|
||||
(New-Point 0.2 0.2 0.4 ([Math]::PI / 2.0)))
|
||||
$cornerOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)))[0].Points
|
||||
Assert-Equal 1 (Get-InterpolatedRunCount $cornerOutput) 'One corner must produce exactly one contiguous Bézier replacement.'
|
||||
Assert-Equal $cornerSource.Count $cornerOutput.Count 'One local replacement must preserve the segment sampling topology.'
|
||||
Assert-True (($cornerOutput[2].X -ne $cornerSource[2].X) -or ($cornerOutput[2].Y -ne $cornerSource[2].Y)) 'The corner sample must be replaced by cubic Bézier geometry.'
|
||||
Assert-PointBitwiseEqual $cornerSource[1] $cornerOutput[1] 'Bézier entry anchor must remain fixed.'
|
||||
Assert-PointBitwiseEqual $cornerSource[3] $cornerOutput[3] 'Bézier exit anchor must remain fixed.'
|
||||
$cornerChordLength = [Math]::Sqrt(
|
||||
[Math]::Pow($cornerSource[3].X - $cornerSource[1].X, 2.0) +
|
||||
[Math]::Pow($cornerSource[3].Y - $cornerSource[1].Y, 2.0))
|
||||
$cornerHandleLength = $cornerChordLength / 3.0
|
||||
$expectedCornerX =
|
||||
0.125 * $cornerSource[1].X +
|
||||
0.375 * ($cornerSource[1].X + $cornerHandleLength) +
|
||||
0.375 * $cornerSource[3].X +
|
||||
0.125 * $cornerSource[3].X
|
||||
$expectedCornerY =
|
||||
0.125 * $cornerSource[1].Y +
|
||||
0.375 * $cornerSource[1].Y +
|
||||
0.375 * ($cornerSource[3].Y - $cornerHandleLength) +
|
||||
0.125 * $cornerSource[3].Y
|
||||
$cornerInterpolated = Get-FirstInterpolatedPoint $cornerOutput
|
||||
Assert-Near $expectedCornerX $cornerInterpolated.X 0.000000000001 'Bézier control handles must use the local endpoint chord length for X geometry.'
|
||||
Assert-Near $expectedCornerY $cornerInterpolated.Y 0.000000000001 'Bézier control handles must use the local endpoint chord length for Y geometry.'
|
||||
|
||||
# Adjacent corner windows touch/overlap and must become one merged cubic replacement, not two sequential fits.
|
||||
$overlappingSource = @(
|
||||
(New-Point 0.0 0.0 0.0 0.0),
|
||||
(New-Point 0.1 0.0 0.1 0.0),
|
||||
(New-Point 0.2 0.0 0.2 0.0),
|
||||
(New-Point 0.2 0.1 0.3 ([Math]::PI / 2.0)),
|
||||
(New-Point 0.3 0.1 0.4 0.0),
|
||||
(New-Point 0.4 0.1 0.5 0.0))
|
||||
$overlappingOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $overlappingSource)))[0].Points
|
||||
Assert-Equal 1 (Get-InterpolatedRunCount $overlappingOutput) 'Touching local corner windows must merge into exactly one Bézier replacement.'
|
||||
Assert-PointBitwiseEqual $overlappingSource[1] $overlappingOutput[1] 'Merged Bézier entry anchor must remain fixed.'
|
||||
Assert-PointBitwiseEqual $overlappingSource[4] $overlappingOutput[4] 'Merged Bézier exit anchor must remain fixed.'
|
||||
Assert-True (($overlappingOutput[2].X -ne $overlappingSource[2].X) -or ($overlappingOutput[2].Y -ne $overlappingSource[2].Y)) 'Merged window must replace the first interior corner sample.'
|
||||
Assert-True (($overlappingOutput[3].X -ne $overlappingSource[3].X) -or ($overlappingOutput[3].Y -ne $overlappingSource[3].Y)) 'Merged window must replace the second interior corner sample.'
|
||||
|
||||
# Safe bounded policy: decline an entire connected set when its merged interval exceeds the cap.
|
||||
# The two candidate windows below are each 0.20 m, but their merged 0.30 m interval must not
|
||||
# produce one over-length curve or be split into new unrequested joins.
|
||||
$overCapMergedOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $overlappingSource)) 0.02 ([Math]::PI / 18.0) 0.20)[0].Points
|
||||
Assert-Equal 0 (Get-InterpolatedRunCount $overCapMergedOutput) 'An oversized connected Bézier window set must be declined instead of emitting an over-cap replacement.'
|
||||
for ($index = 0; $index -lt $overlappingSource.Count; $index++) {
|
||||
Assert-PointBitwiseEqual $overlappingSource[$index] $overCapMergedOutput[$index] 'Declining an oversized connected set must preserve its anchors.'
|
||||
}
|
||||
|
||||
# Samples outside a local window must remain bitwise unchanged rather than be globally re-fit.
|
||||
$isolatedSource = @(
|
||||
(New-Point 0.0 0.0 0.0 0.0),
|
||||
(New-Point 0.1 0.0 0.1 0.0),
|
||||
(New-Point 0.2 0.0 0.2 0.0),
|
||||
(New-Point 0.3 0.0 0.3 0.0),
|
||||
(New-Point 0.3 0.1 0.4 ([Math]::PI / 2.0)),
|
||||
(New-Point 0.3 0.2 0.5 ([Math]::PI / 2.0)),
|
||||
(New-Point 0.3 0.3 0.6 ([Math]::PI / 2.0)))
|
||||
$isolatedOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $isolatedSource)))[0].Points
|
||||
foreach ($index in @(0, 1, 2, 5, 6)) {
|
||||
Assert-PointBitwiseEqual $isolatedSource[$index] (Get-PointAtArcLength $isolatedOutput $isolatedSource[$index].ArcLength) 'Samples outside a Bézier window must remain bitwise unchanged.'
|
||||
}
|
||||
|
||||
# Direction segments stay independent; segment endpoints and the gear-switch anchor are fixed.
|
||||
$reverseSource = @(
|
||||
(New-Point 0.2 0.2 0.0 ([Math]::PI / 2.0) 0.10 $true),
|
||||
(New-Point 0.2 0.1 0.1 ([Math]::PI / 2.0)),
|
||||
(New-Point 0.2 0.0 0.2 ([Math]::PI / 2.0)))
|
||||
$switchOutput = @(Invoke-Smoothing @(
|
||||
(New-DirectionSegment 0 $forward $cornerSource $false $true),
|
||||
(New-DirectionSegment 1 $reverse $reverseSource $true $false)))
|
||||
Assert-Equal 2 $switchOutput.Count 'Bézier smoothing must retain separate forward and reverse direction segments.'
|
||||
Assert-True $switchOutput[0].EndsAtGearSwitch 'The forward segment must retain its gear-switch boundary flag.'
|
||||
Assert-True $switchOutput[1].StartsAtGearSwitch 'The reverse segment must retain its gear-switch boundary flag.'
|
||||
Assert-PointBitwiseEqual $cornerSource[0] $switchOutput[0].Points[0] 'Segment start endpoint must remain fixed.'
|
||||
Assert-PointBitwiseEqual $cornerSource[$cornerSource.Count - 1] $switchOutput[0].Points[$switchOutput[0].Points.Count - 1] 'Segment end endpoint must remain fixed.'
|
||||
Assert-PointBitwiseEqual $reverseSource[0] $switchOutput[1].Points[0] 'Gear-switch point must remain fixed.'
|
||||
|
||||
# The immutable options each change only their own local behavior.
|
||||
$thresholdOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 1.70)[0].Points
|
||||
Assert-Equal 0 (Get-InterpolatedRunCount $thresholdOutput) 'A non-default heading threshold above the corner angle must suppress only corner detection.'
|
||||
$windowOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 ([Math]::PI / 18.0) 0.15)[0].Points
|
||||
Assert-Equal 0 (Get-InterpolatedRunCount $windowOutput) 'A non-default maximum window shorter than the local connection must suppress only that window.'
|
||||
$shortHandleOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 ([Math]::PI / 18.0) 0.60 0.10)[0].Points
|
||||
$longHandleOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 ([Math]::PI / 18.0) 0.60 0.60)[0].Points
|
||||
Assert-Equal 1 (Get-InterpolatedRunCount $shortHandleOutput) 'Changing handle ratio must not change detected window topology.'
|
||||
Assert-Equal 1 (Get-InterpolatedRunCount $longHandleOutput) 'Changing handle ratio must not change detected window topology.'
|
||||
$shortHandlePoint = Get-FirstInterpolatedPoint $shortHandleOutput
|
||||
$longHandlePoint = Get-FirstInterpolatedPoint $longHandleOutput
|
||||
Assert-True (($shortHandlePoint.X -ne $longHandlePoint.X) -or ($shortHandlePoint.Y -ne $longHandlePoint.Y)) 'A non-default handle ratio must change only the local Bézier geometry.'
|
||||
|
||||
# Parameter-matched local arc-length reference comparison must reject excess displacement as retryable and publish no geometry.
|
||||
$infeasible = Invoke-Candidate @((New-DirectionSegment 0 $forward $cornerSource)) 0.095
|
||||
Assert-Equal 'RetryableInfeasible' (Get-PropertyValue $infeasible 'Status').ToString() 'Exceeded local arc-length displacement must be retryable, not terminal.'
|
||||
Assert-True (-not (Get-PropertyValue $infeasible 'Succeeded')) 'An infeasible Bézier curve must not be executable.'
|
||||
Assert-Equal 0 (Get-PropertyValue $infeasible 'Segments').Count 'A retryable Bézier infeasibility must publish no executable geometry.'
|
||||
|
||||
# This nonuniform, offset window evaluates its only interior point at t=0.25 and local s=6.
|
||||
# A wrong global/index mapping would instead compare to s=5 and accept the 0.50 m clearance;
|
||||
# the required local-arc reference at s=6 must reject the roughly 0.65 m displacement.
|
||||
$nonuniformOffsetSource = @(
|
||||
(New-Point 0.0 0.0 0.0 0.0 0.50),
|
||||
(New-Point 1.0 0.0 4.0 0.0 0.50),
|
||||
(New-Point 2.0 0.0 5.0 0.0 0.50),
|
||||
(New-Point 3.0 0.0 6.0 0.0 0.50),
|
||||
(New-Point 3.0 1.0 9.0 ([Math]::PI / 2.0) 0.50),
|
||||
(New-Point 3.0 2.0 20.0 ([Math]::PI / 2.0) 0.50))
|
||||
$nonuniformOffsetInfeasible = Invoke-Candidate @((New-DirectionSegment 0 $forward $nonuniformOffsetSource)) 0.0 ([Math]::PI / 18.0) 5.0
|
||||
Assert-Equal 'RetryableInfeasible' (Get-PropertyValue $nonuniformOffsetInfeasible 'Status').ToString() 'A nonuniform offset window must use local arc-length mapping for retryable clearance rejection.'
|
||||
Assert-True (-not (Get-PropertyValue $nonuniformOffsetInfeasible 'Succeeded')) 'The nonuniform local-arc infeasibility must not be executable.'
|
||||
Assert-Equal 0 (Get-PropertyValue $nonuniformOffsetInfeasible 'Segments').Count 'The nonuniform local-arc infeasibility must publish no geometry.'
|
||||
|
||||
Write-Output 'Path smoothing local cubic Bézier checks passed.'
|
||||
@@ -0,0 +1,286 @@
|
||||
param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
|
||||
|
||||
$ErrorActionPreference = 'Stop'
|
||||
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
|
||||
|
||||
function Assert-True($Actual, [string]$Message) {
|
||||
if (-not $Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-Equal($Expected, $Actual, [string]$Message) {
|
||||
if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" }
|
||||
}
|
||||
|
||||
function Assert-Near([double]$Expected, [double]$Actual, [double]$Tolerance, [string]$Message) {
|
||||
if ([Math]::Abs($Expected - $Actual) -gt $Tolerance) {
|
||||
throw "$Message Expected=$Expected Actual=$Actual Tolerance=$Tolerance"
|
||||
}
|
||||
}
|
||||
|
||||
function Get-RequiredType([string]$Name) {
|
||||
return $assembly.GetType($Name, $true)
|
||||
}
|
||||
|
||||
function Get-PropertyValue($Instance, [string]$Name) {
|
||||
$property = $Instance.GetType().GetProperty($Name, [Reflection.BindingFlags]'Instance,Public,NonPublic')
|
||||
Assert-True ($null -ne $property) ("Missing property: " + $Name)
|
||||
return $property.GetValue($Instance)
|
||||
}
|
||||
|
||||
function New-Point(
|
||||
[double]$X,
|
||||
[double]$Y,
|
||||
[double]$ArcLength,
|
||||
[double]$Heading,
|
||||
[double]$BodyClearance,
|
||||
[bool]$IsGearSwitch = $false) {
|
||||
return [Activator]::CreateInstance($pointType, @(
|
||||
$X, $Y, $ArcLength, $Heading, $Heading, $BodyClearance, $IsGearSwitch, $anchor))
|
||||
}
|
||||
|
||||
function New-DirectionSegment(
|
||||
[int]$Index,
|
||||
$Direction,
|
||||
[object[]]$Points,
|
||||
[bool]$StartsAtGearSwitch = $false,
|
||||
[bool]$EndsAtGearSwitch = $false) {
|
||||
$typedPoints = [Array]::CreateInstance($pointType, $Points.Count)
|
||||
for ($pointIndex = 0; $pointIndex -lt $Points.Count; $pointIndex++) {
|
||||
$typedPoints.SetValue($Points[$pointIndex], $pointIndex)
|
||||
}
|
||||
return [Activator]::CreateInstance($segmentType, @(
|
||||
$Index, $Direction, $typedPoints, $StartsAtGearSwitch, $EndsAtGearSwitch))
|
||||
}
|
||||
|
||||
function New-EmptyMap {
|
||||
$request = [Activator]::CreateInstance($mapRequestType)
|
||||
$request.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000))
|
||||
$request.ResolutionMm = [single]50
|
||||
$request.AllowExplicitEmptyMap = $true
|
||||
$map = [Activator]::CreateInstance($mapFactoryType).Create($request).Map
|
||||
Assert-True ($null -ne $map) 'B-spline test must create an explicit empty planning map.'
|
||||
return $map
|
||||
}
|
||||
|
||||
function New-AlgorithmInput(
|
||||
[object[]]$Segments,
|
||||
[double]$ReserveMeters,
|
||||
[double]$EndpointTangentScale = (1.0 / 3.0)) {
|
||||
$typedSegments = [Array]::CreateInstance($segmentType, $Segments.Count)
|
||||
for ($index = 0; $index -lt $Segments.Count; $index++) {
|
||||
$typedSegments.SetValue($Segments[$index], $index)
|
||||
}
|
||||
$preparedPath = [Activator]::CreateInstance($preparedPathType, [object[]]@(,$typedSegments))
|
||||
$vehicle = [Activator]::CreateInstance($vehicleType)
|
||||
$vehicle.LengthMeters = [double]0.20
|
||||
$vehicle.WidthMeters = [double]0.20
|
||||
$vehicle.SafetyMarginMeters = [double]0.0
|
||||
$vehicle.MaximumCurvaturePerMeter = [double]100.0
|
||||
$vehicle.MinimumTurningRadiusMeters = [double]0.01
|
||||
$configuration = [Activator]::CreateInstance($configurationType)
|
||||
$configuration.CubicBSpline.EndpointTangentScale = $EndpointTangentScale
|
||||
$options = $optionsConstructor.Invoke(@($configuration))
|
||||
return $inputConstructor.Invoke(@($preparedPath, (New-EmptyMap), $vehicle, [double]0.05, $ReserveMeters, $options))
|
||||
}
|
||||
|
||||
function Invoke-Candidate(
|
||||
[object[]]$Segments,
|
||||
[double]$ReserveMeters,
|
||||
[double]$Strength = 1.0,
|
||||
[double]$EndpointTangentScale = (1.0 / 3.0)) {
|
||||
return $smoothMethod.Invoke($smoother, @(
|
||||
(New-AlgorithmInput $Segments $ReserveMeters $EndpointTangentScale), $Strength, [Threading.CancellationToken]::None))
|
||||
}
|
||||
|
||||
function Invoke-Smoothing(
|
||||
[object[]]$Segments,
|
||||
[double]$ReserveMeters,
|
||||
[double]$Strength = 1.0,
|
||||
[double]$EndpointTangentScale = (1.0 / 3.0)) {
|
||||
$candidate = Invoke-Candidate $Segments $ReserveMeters $Strength $EndpointTangentScale
|
||||
Assert-True (Get-PropertyValue $candidate 'Succeeded') 'B-spline smoothing must produce a candidate for the deterministic fixture.'
|
||||
return @(Get-PropertyValue $candidate 'Segments')
|
||||
}
|
||||
|
||||
function Get-PointDistance($Left, $Right) {
|
||||
$deltaX = $Left.X - $Right.X
|
||||
$deltaY = $Left.Y - $Right.Y
|
||||
return [Math]::Sqrt($deltaX * $deltaX + $deltaY * $deltaY)
|
||||
}
|
||||
|
||||
function Get-DistanceToSegment($Point, $Left, $Right) {
|
||||
$deltaX = $Right.X - $Left.X
|
||||
$deltaY = $Right.Y - $Left.Y
|
||||
$lengthSquared = $deltaX * $deltaX + $deltaY * $deltaY
|
||||
if ($lengthSquared -le 0.0) { return Get-PointDistance $Point $Left }
|
||||
$projection = (($Point.X - $Left.X) * $deltaX + ($Point.Y - $Left.Y) * $deltaY) / $lengthSquared
|
||||
$projection = [Math]::Max(0.0, [Math]::Min(1.0, $projection))
|
||||
$closest = New-Object PSObject -Property @{
|
||||
X = $Left.X + $projection * $deltaX
|
||||
Y = $Left.Y + $projection * $deltaY
|
||||
}
|
||||
return Get-PointDistance $Point $closest
|
||||
}
|
||||
|
||||
function Get-DistanceToPolyline($Point, [object[]]$SourcePoints) {
|
||||
$minimum = [double]::PositiveInfinity
|
||||
for ($index = 1; $index -lt $SourcePoints.Count; $index++) {
|
||||
$minimum = [Math]::Min($minimum, (Get-DistanceToSegment $Point $SourcePoints[$index - 1] $SourcePoints[$index]))
|
||||
}
|
||||
return $minimum
|
||||
}
|
||||
|
||||
function Get-TravelAngle($Left, $Right) {
|
||||
return [Math]::Atan2($Right.Y - $Left.Y, $Right.X - $Left.X)
|
||||
}
|
||||
|
||||
function Get-AngleDifference([double]$Left, [double]$Right) {
|
||||
$difference = $Left - $Right
|
||||
while ($difference -gt [Math]::PI) { $difference -= 2.0 * [Math]::PI }
|
||||
while ($difference -lt -[Math]::PI) { $difference += 2.0 * [Math]::PI }
|
||||
return [Math]::Abs($difference)
|
||||
}
|
||||
|
||||
$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
|
||||
$processing = $root + 'Processing.'
|
||||
$algorithms = $root + 'Algorithms.'
|
||||
$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
|
||||
|
||||
$smootherType = Get-RequiredType ($algorithms + 'CubicBSplineSmoother')
|
||||
$pointType = Get-RequiredType ($processing + 'SmoothingPoint2D')
|
||||
$segmentType = Get-RequiredType ($processing + 'PreparedDirectionSegment')
|
||||
$preparedPathType = Get-RequiredType ($processing + 'PreparedPath')
|
||||
$inputType = Get-RequiredType ($algorithms + 'SmoothingAlgorithmInput')
|
||||
$optionsType = Get-RequiredType ($algorithms + 'SmoothingOptionsSnapshot')
|
||||
$interpolatorType = Get-RequiredType ($processing + 'PathReferenceInterpolator')
|
||||
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
|
||||
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
|
||||
$directionType = Get-RequiredType ($coarsePath + 'TravelDirection')
|
||||
$sourceType = Get-RequiredType ($root + 'SmoothedPathPointSource')
|
||||
$boundsType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.MapBoundsMm'
|
||||
$mapType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningGridMap'
|
||||
$mapRequestType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapRequest'
|
||||
$mapFactoryType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapFactory'
|
||||
|
||||
$inputConstructor = $inputType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null,
|
||||
@($preparedPathType, $mapType, $vehicleType, [double], [double], $optionsType), $null)
|
||||
Assert-True ($null -ne $inputConstructor) 'Algorithm input must carry immutable smoothing options and the minimum clearance reserve for per-anchor movement limits.'
|
||||
$optionsConstructor = $optionsType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null, @($configurationType), $null)
|
||||
Assert-True ($null -ne $optionsConstructor) 'B-spline tests must create an immutable options snapshot.'
|
||||
$interpolateMethod = $interpolatorType.GetMethod('TryInterpolateByArcLength', [Reflection.BindingFlags]'Static,Public,NonPublic')
|
||||
Assert-True ($null -ne $interpolateMethod) 'PathReferenceInterpolator must expose arc-length interpolation.'
|
||||
$smoother = [Activator]::CreateInstance($smootherType, $true)
|
||||
$smoothMethod = $smootherType.GetMethod('Smooth', [Reflection.BindingFlags]'Instance,Public')
|
||||
Assert-True ($null -ne $smoothMethod) 'CubicBSplineSmoother must implement the internal smoother contract.'
|
||||
Assert-Equal 'CubicBSpline' $smoother.Method.ToString() 'B-spline smoother must identify its public smoothing method.'
|
||||
|
||||
$forward = [Enum]::Parse($directionType, 'Forward')
|
||||
$reverse = [Enum]::Parse($directionType, 'Reverse')
|
||||
$anchor = [Enum]::Parse($sourceType, 'Anchor')
|
||||
|
||||
# Arc-length interpolation must bracket by local arc rather than by sample index.
|
||||
$nonUniformPoints = [Array]::CreateInstance($pointType, 4)
|
||||
$nonUniformPoints.SetValue((New-Point 0.0 0.0 0.000 0.0 1.0), 0)
|
||||
$nonUniformPoints.SetValue((New-Point 5.0 0.0 0.050 0.1 0.9), 1)
|
||||
$nonUniformPoints.SetValue((New-Point 10.0 0.0 0.100 0.2 0.8), 2)
|
||||
$nonUniformPoints.SetValue((New-Point 20.0 0.0 0.125 0.3 0.7), 3)
|
||||
$interpolateArguments = [object[]]@($nonUniformPoints, [double]0.1125, $null, $null)
|
||||
Assert-True $interpolateMethod.Invoke($null, $interpolateArguments) 'Arc-length interpolation must accept a target within the final non-uniform interval.'
|
||||
$arcReference = $interpolateArguments[2]
|
||||
Assert-Near 15.0 $arcReference.X 0.000000001 'Target arc length 0.1125 must lie halfway through the final 0.100-0.125 interval, independent of point count.'
|
||||
Assert-Near 0.1125 $arcReference.ArcLength 0.000000001 'Arc-length interpolation must preserve the requested target arc length.'
|
||||
Assert-Near 0.25 $arcReference.Heading 0.000000001 'Arc-length interpolation must linearly interpolate heading.'
|
||||
Assert-Near 0.75 $arcReference.BodyClearance 0.000000001 'Arc-length interpolation must linearly interpolate clearance.'
|
||||
|
||||
# Straight samples are returned exactly, so a straight is never distorted or densified.
|
||||
$straightSource = @(
|
||||
(New-Point 0.0 0.0 0.0 0.0 0.03),
|
||||
(New-Point 1.0 0.0 1.0 0.0 0.03),
|
||||
(New-Point 2.0 0.0 2.0 0.0 0.03),
|
||||
(New-Point 3.0 0.0 3.0 0.0 0.03))
|
||||
$straightResult = Invoke-Smoothing @((New-DirectionSegment 0 $forward $straightSource)) 0.02
|
||||
Assert-Equal 1 $straightResult.Count 'A single direction segment must produce exactly one candidate segment.'
|
||||
Assert-Equal $straightSource.Count $straightResult[0].Points.Count 'A straight must retain its original samples.'
|
||||
for ($index = 0; $index -lt $straightSource.Count; $index++) {
|
||||
Assert-Near $straightSource[$index].X $straightResult[0].Points[$index].X 0.0 'Straight X coordinates must remain exact.'
|
||||
Assert-Near $straightSource[$index].Y $straightResult[0].Points[$index].Y 0.0 'Straight Y coordinates must remain exact.'
|
||||
}
|
||||
|
||||
# A five-anchor corner uses the preprocessor's 0.05 m sampling scale. It must retain exact endpoint poses,
|
||||
# follow endpoint travel tangents, turn continuously, and stay within the per-anchor clearance reserve radius.
|
||||
$cornerSource = @(
|
||||
(New-Point 0.0 0.0 0.0 0.0 0.08),
|
||||
(New-Point 0.05 0.0 0.05 0.0 0.08),
|
||||
(New-Point 0.10 0.0 0.10 0.0 0.08),
|
||||
(New-Point 0.10 0.05 0.15 ([Math]::PI / 2.0) 0.08),
|
||||
(New-Point 0.10 0.10 0.20 ([Math]::PI / 2.0) 0.08))
|
||||
$cornerResult = Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02
|
||||
$cornerPoints = @($cornerResult[0].Points)
|
||||
Assert-True ($cornerPoints.Count -gt $cornerSource.Count) 'A non-straight B-spline candidate must provide sampled curve geometry.'
|
||||
$cornerStart = $cornerPoints[0]
|
||||
$cornerEnd = $cornerPoints[$cornerPoints.Count - 1]
|
||||
Assert-Near $cornerSource[0].X $cornerStart.X 0.0 'B-spline start X must be exact.'
|
||||
Assert-Near $cornerSource[0].Y $cornerStart.Y 0.0 'B-spline start Y must be exact.'
|
||||
Assert-Near $cornerSource[$cornerSource.Count - 1].X $cornerEnd.X 0.0 'B-spline end X must be exact.'
|
||||
Assert-Near $cornerSource[$cornerSource.Count - 1].Y $cornerEnd.Y 0.0 'B-spline end Y must be exact.'
|
||||
Assert-Near 0.0 (Get-AngleDifference (Get-TravelAngle $cornerPoints[0] $cornerPoints[1]) 0.0) 0.02 'B-spline start travel tangent must follow the supplied forward heading.'
|
||||
Assert-Near 0.0 (Get-AngleDifference (Get-TravelAngle $cornerPoints[$cornerPoints.Count - 2] $cornerPoints[$cornerPoints.Count - 1]) ([Math]::PI / 2.0)) 0.02 'B-spline end travel tangent must follow the supplied forward heading.'
|
||||
for ($index = 2; $index -lt $cornerPoints.Count; $index++) {
|
||||
$previousAngle = Get-TravelAngle $cornerPoints[$index - 2] $cornerPoints[$index - 1]
|
||||
$currentAngle = Get-TravelAngle $cornerPoints[$index - 1] $cornerPoints[$index]
|
||||
Assert-True ((Get-AngleDifference $previousAngle $currentAngle) -lt 0.08) 'B-spline corner samples must turn without a tangent discontinuity.'
|
||||
}
|
||||
|
||||
# Endpoint tangent scale is an immutable option and must influence the clamped B-spline endpoint handle.
|
||||
$shortHandlePoints = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 1.0 0.20)[0].Points
|
||||
$longHandlePoints = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $cornerSource)) 0.02 1.0 0.60)[0].Points
|
||||
Assert-True (($longHandlePoints[2].X - $shortHandlePoints[2].X) -gt 0.0001) 'A custom endpoint tangent scale must change the B-spline start handle and early curve samples.'
|
||||
|
||||
# Every evaluated point may deviate only by BodyClearance - reserve, never by raw BodyClearance.
|
||||
$allowedRadius = 0.06
|
||||
foreach ($point in $cornerPoints) {
|
||||
Assert-True ((Get-DistanceToPolyline $point $cornerSource) -le ($allowedRadius + 0.000000001)) 'Every B-spline displacement must stay inside the per-anchor clearance reserve radius.'
|
||||
}
|
||||
|
||||
# A 0.035 m reserve radius must reject this corner by its parameter-matched source reference,
|
||||
# even though the candidate's nearest-polyline distance is smaller than 0.035 m.
|
||||
$parameterMatchedReserveCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $cornerSource)) 0.045
|
||||
Assert-True (-not (Get-PropertyValue $parameterMatchedReserveCandidate 'Succeeded')) 'A medium reserve must reject B-spline geometry that exceeds its parameter-matched movement radius.'
|
||||
|
||||
# A tight reserve may not publish an evaluated B-spline that leaves its 0.005 m movement radius.
|
||||
$tightReserveCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $cornerSource)) 0.075
|
||||
|
||||
# A tight endpoint circle that cannot meet the heading=0.2 rad tangent ray must fail, not silently rotate the handle.
|
||||
$misalignedHeadingSource = @(
|
||||
(New-Point 0.0 0.0 0.0 0.2 0.08),
|
||||
(New-Point 0.05 0.0 0.05 0.0 0.08),
|
||||
(New-Point 0.10 0.0 0.10 0.0 0.08),
|
||||
(New-Point 0.10 0.05 0.15 ([Math]::PI / 2.0) 0.08),
|
||||
(New-Point 0.10 0.10 0.20 ([Math]::PI / 2.0) 0.08))
|
||||
$misalignedHeadingCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $misalignedHeadingSource)) 0.075
|
||||
$requiredFailures = New-Object System.Collections.Generic.List[string]
|
||||
if (Get-PropertyValue $tightReserveCandidate 'Succeeded') {
|
||||
[void]$requiredFailures.Add('A tight reserve published an evaluated candidate outside its permitted movement radius.')
|
||||
}
|
||||
if (Get-PropertyValue $misalignedHeadingCandidate 'Succeeded') {
|
||||
[void]$requiredFailures.Add('A tight reserve silently accepted an endpoint handle that cannot follow the supplied travel tangent.')
|
||||
}
|
||||
Assert-Equal 0 $requiredFailures.Count ([string]::Join(' ', $requiredFailures))
|
||||
|
||||
# Adjacent direction segments retain their duplicated switch pose and independent topology; no fit may cross the switch.
|
||||
$reverseSource = @(
|
||||
(New-Point 0.10 0.10 0.0 ([Math]::PI / 2.0) 0.08 $true),
|
||||
(New-Point 0.10 0.05 0.05 ([Math]::PI / 2.0) 0.08),
|
||||
(New-Point 0.10 0.0 0.10 ([Math]::PI / 2.0) 0.08))
|
||||
$switchResult = Invoke-Smoothing @(
|
||||
(New-DirectionSegment 0 $forward $cornerSource $false $true),
|
||||
(New-DirectionSegment 1 $reverse $reverseSource $true $false)) 0.02
|
||||
Assert-Equal 2 $switchResult.Count 'B-spline smoothing must preserve each direction segment boundary.'
|
||||
Assert-True $switchResult[0].EndsAtGearSwitch 'The forward segment must retain its gear-switch boundary flag.'
|
||||
Assert-True $switchResult[1].StartsAtGearSwitch 'The reverse segment must retain its gear-switch boundary flag.'
|
||||
$switchLeft = $switchResult[0].Points[$switchResult[0].Points.Count - 1]
|
||||
$switchRight = $switchResult[1].Points[0]
|
||||
Assert-Near $switchLeft.X $switchRight.X 0.0 'B-spline smoothing must retain the duplicated gear-switch X pose.'
|
||||
Assert-Near $switchLeft.Y $switchRight.Y 0.0 'B-spline smoothing must retain the duplicated gear-switch Y pose.'
|
||||
|
||||
Write-Output 'Path smoothing cubic B-spline checks passed.'
|
||||
@@ -0,0 +1,230 @@
|
||||
param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
|
||||
|
||||
$ErrorActionPreference = 'Stop'
|
||||
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
|
||||
|
||||
function Assert-True($Actual, [string]$Message) {
|
||||
if (-not $Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-Equal($Expected, $Actual, [string]$Message) {
|
||||
if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" }
|
||||
}
|
||||
|
||||
function Assert-Near([double]$Expected, [double]$Actual, [double]$Tolerance, [string]$Message) {
|
||||
if ([Math]::Abs($Expected - $Actual) -gt $Tolerance) {
|
||||
throw "$Message Expected=$Expected Actual=$Actual Tolerance=$Tolerance"
|
||||
}
|
||||
}
|
||||
|
||||
function Get-RequiredType([string]$Name) {
|
||||
return $assembly.GetType($Name, $true)
|
||||
}
|
||||
|
||||
function New-Map {
|
||||
$mapRequest = [Activator]::CreateInstance($mapRequestType)
|
||||
$mapRequest.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000))
|
||||
$mapRequest.ResolutionMm = [single]50
|
||||
$mapRequest.AllowExplicitEmptyMap = $true
|
||||
$map = [Activator]::CreateInstance($mapFactoryType).Create($mapRequest).Map
|
||||
Assert-True ($null -ne $map) 'Comparison test must create a planning map.'
|
||||
return $map
|
||||
}
|
||||
|
||||
function New-Vehicle {
|
||||
$vehicle = [Activator]::CreateInstance($vehicleType)
|
||||
$vehicle.LengthMeters = [double]0.20
|
||||
$vehicle.WidthMeters = [double]0.20
|
||||
$vehicle.SafetyMarginMeters = [double]0.0
|
||||
$vehicle.MaximumCurvaturePerMeter = [double]100.0
|
||||
return $vehicle
|
||||
}
|
||||
|
||||
function New-CoarsePoint([double]$X, [double]$Y, [double]$ArcLength) {
|
||||
return [Activator]::CreateInstance($coarsePointType, @(
|
||||
$X, $Y, [double]0.0, [double]0.0, $ArcLength, $forward,
|
||||
[double]0.0, [double]1.0, $false, $coarseAnchor))
|
||||
}
|
||||
|
||||
function New-SmoothingRequest {
|
||||
$points = [Array]::CreateInstance($coarsePointType, 2)
|
||||
$points.SetValue((New-CoarsePoint 0.5 0.5 0.0), 0)
|
||||
$points.SetValue((New-CoarsePoint 1.5 0.5 1.0), 1)
|
||||
$segments = [Array]::CreateInstance($coarseSegmentType, 1)
|
||||
$segments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(0, $forward, 0, 1, $false, $false)), 0)
|
||||
$configuration = [Activator]::CreateInstance($configurationType)
|
||||
return [Activator]::CreateInstance($smoothingRequestType, @($points, $segments, (New-Map), (New-Vehicle), $configuration))
|
||||
}
|
||||
|
||||
function New-CornerSmoothingRequest {
|
||||
$points = [Array]::CreateInstance($coarsePointType, 4)
|
||||
$points.SetValue((New-CoarsePoint 0.5 0.5 0.0), 0)
|
||||
$points.SetValue((New-CoarsePoint 1.0 0.5 0.5), 1)
|
||||
$points.SetValue((New-CoarsePoint 1.0 1.0 1.0), 2)
|
||||
$points.SetValue((New-CoarsePoint 1.5 1.0 1.5), 3)
|
||||
$segments = [Array]::CreateInstance($coarseSegmentType, 1)
|
||||
$segments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(0, $forward, 0, 3, $false, $false)), 0)
|
||||
$configuration = [Activator]::CreateInstance($configurationType)
|
||||
return [Activator]::CreateInstance($smoothingRequestType, @($points, $segments, (New-Map), (New-Vehicle), $configuration))
|
||||
}
|
||||
|
||||
function New-Metrics(
|
||||
[double]$VariationEnergy,
|
||||
[double]$PeakCurvature,
|
||||
[double]$MinimumClearance,
|
||||
[double]$LengthChangePercent) {
|
||||
return [Activator]::CreateInstance($metricsType, @(
|
||||
$true, [double]1.0, $PeakCurvature, [double]0.0, [double]0.0, $VariationEnergy,
|
||||
$MinimumClearance, $LengthChangePercent, [double]0.0, [double]0.0, [double]0.0))
|
||||
}
|
||||
|
||||
function New-Timing([double]$MedianMilliseconds) {
|
||||
[double[]]$samples = @($MedianMilliseconds, $MedianMilliseconds, $MedianMilliseconds, $MedianMilliseconds, $MedianMilliseconds)
|
||||
return [Activator]::CreateInstance($timingType, @($samples, $true, ''))
|
||||
}
|
||||
|
||||
function New-Entry(
|
||||
$Method,
|
||||
[double]$VariationEnergy,
|
||||
[double]$PeakCurvature,
|
||||
[double]$MinimumClearance,
|
||||
[double]$LengthChangePercent,
|
||||
[double]$MedianMilliseconds) {
|
||||
[object[]]$arguments = @(
|
||||
$Method, $successStatus, (New-Metrics $VariationEnergy $PeakCurvature $MinimumClearance $LengthChangePercent),
|
||||
(New-Timing $MedianMilliseconds), ('synthetic-' + $Method.ToString()), '')
|
||||
return [Activator]::CreateInstance($entryType, $arguments)
|
||||
}
|
||||
|
||||
$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
|
||||
$comparison = $root + 'Comparison.'
|
||||
$facade = $root + 'Facade.'
|
||||
$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
|
||||
$mapping = 'MultiWheelC.TrajectoryPlanning.Mapping.'
|
||||
|
||||
$comparisonServiceType = Get-RequiredType ($facade + 'PathSmoothingComparisonService')
|
||||
$comparisonRequestType = Get-RequiredType ($comparison + 'PathSmoothingComparisonRequest')
|
||||
$comparisonResultType = Get-RequiredType ($comparison + 'PathSmoothingComparisonResult')
|
||||
$entryType = Get-RequiredType ($comparison + 'PathSmoothingComparisonEntry')
|
||||
$timingType = Get-RequiredType ($comparison + 'SmoothingTimingSummary')
|
||||
$rankerType = Get-RequiredType ($comparison + 'SmoothingMethodRanker')
|
||||
$smoothingRequestType = Get-RequiredType ($root + 'PathSmoothingRequest')
|
||||
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
|
||||
$metricsType = Get-RequiredType ($root + 'PathQualityMetrics')
|
||||
$methodType = Get-RequiredType ($root + 'SmoothingMethod')
|
||||
$statusType = Get-RequiredType ($root + 'PathSmoothingStatus')
|
||||
$smoothingResultType = Get-RequiredType ($root + 'PathSmoothingResult')
|
||||
$diagnosticsType = Get-RequiredType ($root + 'PathSmoothingDiagnostics')
|
||||
$coarsePointType = Get-RequiredType ($coarsePath + 'CoarsePathPoint')
|
||||
$coarseSegmentType = Get-RequiredType ($coarsePath + 'PathSegment')
|
||||
$directionType = Get-RequiredType ($coarsePath + 'TravelDirection')
|
||||
$coarsePointSourceType = Get-RequiredType ($coarsePath + 'CoarsePathPointSource')
|
||||
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
|
||||
$boundsType = Get-RequiredType ($mapping + 'MapBoundsMm')
|
||||
$mapRequestType = Get-RequiredType ($mapping + 'PlanningMapRequest')
|
||||
$mapFactoryType = Get-RequiredType ($mapping + 'PlanningMapFactory')
|
||||
|
||||
$forward = [Enum]::Parse($directionType, 'Forward')
|
||||
$coarseAnchor = [Enum]::Parse($coarsePointSourceType, 'Start')
|
||||
$cubicBSpline = [Enum]::Parse($methodType, 'CubicBSpline')
|
||||
$localCubicBezier = [Enum]::Parse($methodType, 'LocalCubicBezier')
|
||||
$piecewiseQuintic = [Enum]::Parse($methodType, 'PiecewiseQuintic')
|
||||
$successStatus = [Enum]::Parse($statusType, 'Success')
|
||||
|
||||
Assert-True $comparisonServiceType.IsPublic 'Comparison service must be public.'
|
||||
$compareMethod = $comparisonServiceType.GetMethod('Compare', [Type[]]@($comparisonRequestType, [Threading.CancellationToken]))
|
||||
Assert-True ($null -ne $compareMethod) 'Comparison service must expose Compare(PathSmoothingComparisonRequest, CancellationToken).'
|
||||
Assert-Equal $comparisonResultType $compareMethod.ReturnType 'Compare must return PathSmoothingComparisonResult.'
|
||||
|
||||
$methods = [Array]::CreateInstance($methodType, 3)
|
||||
$methods.SetValue($cubicBSpline, 0)
|
||||
$methods.SetValue($localCubicBezier, 1)
|
||||
$methods.SetValue($piecewiseQuintic, 2)
|
||||
$comparisonRequest = [Activator]::CreateInstance($comparisonRequestType, @((New-SmoothingRequest), $methods))
|
||||
$comparisonService = [Activator]::CreateInstance($comparisonServiceType)
|
||||
$result = $compareMethod.Invoke($comparisonService, @($comparisonRequest, [Threading.CancellationToken]::None))
|
||||
|
||||
Assert-True ($null -ne $result.RawPathBaseline) 'Comparison must publish a separately analyzed raw-path baseline.'
|
||||
Assert-True $result.RawPathBaseline.IsRawPathBaseline 'Raw baseline must be explicitly marked and excluded from candidates.'
|
||||
Assert-Equal 3 $result.Entries.Count 'Comparison must contain exactly one entry for every requested method.'
|
||||
Assert-True ($null -ne $result.RecommendedMethod) 'A comparison with feasible methods must select a recommendation.'
|
||||
foreach ($entry in $result.Entries) {
|
||||
Assert-True (-not $entry.IsRawPathBaseline) 'Candidate entries must not be marked as the raw baseline.'
|
||||
Assert-Equal 5 $entry.Timing.MeasuredElapsedMilliseconds.Count 'Warm-up must be excluded and exactly five measurements retained.'
|
||||
Assert-True $entry.Timing.IsDeterministic 'Repeated deterministic smoothing geometry must remain eligible for recommendation.'
|
||||
Assert-True (-not [string]::IsNullOrWhiteSpace($entry.StableGeometryDigest)) 'Every measured candidate must expose a stable geometry digest.'
|
||||
}
|
||||
|
||||
$fromMeasurementsMethod = $timingType.GetMethod('FromMeasurements')
|
||||
Assert-True ($null -ne $fromMeasurementsMethod) 'Timing summary must analyze the five measured outputs for deterministic geometry.'
|
||||
$failureFactory = $smoothingResultType.GetMethod('Failure')
|
||||
$invalidInputStatus = [Enum]::Parse($statusType, 'InvalidInput')
|
||||
$infeasibleStatus = [Enum]::Parse($statusType, 'Infeasible')
|
||||
$inconsistentResults = [Array]::CreateInstance($smoothingResultType, 5)
|
||||
for ($index = 0; $index -lt 5; $index++) {
|
||||
$status = if ($index -eq 4) { $infeasibleStatus } else { $invalidInputStatus }
|
||||
[object[]]$failureArguments = New-Object object[] 2
|
||||
$failureArguments[0] = $status
|
||||
$failureArguments[1] = [Activator]::CreateInstance($diagnosticsType)
|
||||
$inconsistentResults.SetValue($failureFactory.Invoke($null, $failureArguments), $index)
|
||||
}
|
||||
[object[]]$timingArguments = New-Object object[] 2
|
||||
$timingArguments[0] = [double[]]@(1.0, 2.0, 3.0, 4.0, 5.0)
|
||||
$timingArguments[1] = $inconsistentResults
|
||||
$nonDeterministicTiming = $fromMeasurementsMethod.Invoke($null, $timingArguments)
|
||||
Assert-True (-not $nonDeterministicTiming.IsDeterministic) 'A status, point-count, segment-count, or digest mismatch must be non-deterministic.'
|
||||
Assert-True (-not [string]::IsNullOrWhiteSpace($nonDeterministicTiming.Diagnostic)) 'Non-deterministic measurements must publish a stable diagnostic.'
|
||||
|
||||
# All published comparison metrics must be normalized against the separately analyzed raw baseline.
|
||||
$cornerMethods = [Array]::CreateInstance($methodType, 1)
|
||||
$cornerMethods.SetValue($cubicBSpline, 0)
|
||||
$cornerRequest = [Activator]::CreateInstance($comparisonRequestType, @((New-CornerSmoothingRequest), $cornerMethods))
|
||||
$cornerResult = $compareMethod.Invoke($comparisonService, @($cornerRequest, [Threading.CancellationToken]::None))
|
||||
$cornerEntry = $cornerResult.Entries[0]
|
||||
Assert-Equal 'Success' $cornerEntry.Status.ToString() 'The unconstrained empty-map corner fixture must produce a B-spline comparison candidate.'
|
||||
$expectedLengthChange = (($cornerEntry.Path[$cornerEntry.Path.Count - 1].ArcLength - $cornerResult.RawPathBaseline.Metrics.PathLengthMeters) /
|
||||
$cornerResult.RawPathBaseline.Metrics.PathLengthMeters) * 100.0
|
||||
Assert-Near $expectedLengthChange $cornerEntry.Metrics.LengthChangePercent 0.000001 'Candidate length change must be normalized relative to the raw baseline.'
|
||||
|
||||
# The ranker must apply every public tie-break in order. Each pair ties all prior criteria.
|
||||
$entryListType = [Collections.Generic.IReadOnlyList``1].MakeGenericType(@($entryType))
|
||||
$rankMethod = $rankerType.GetMethod('Rank', [Type[]]@($entryListType))
|
||||
Assert-True ($null -ne $rankMethod) 'SmoothingMethodRanker must expose Rank(IReadOnlyList<PathSmoothingComparisonEntry>).'
|
||||
|
||||
function Assert-Rank($ExpectedMethod, [object[]]$Entries, [string]$Message) {
|
||||
$typedEntries = [Array]::CreateInstance($entryType, $Entries.Count)
|
||||
for ($index = 0; $index -lt $Entries.Count; $index++) { $typedEntries.SetValue($Entries[$index], $index) }
|
||||
[object[]]$invokeArguments = New-Object object[] 1
|
||||
$invokeArguments[0] = $typedEntries
|
||||
$actual = $rankMethod.Invoke($null, $invokeArguments)
|
||||
Assert-Equal $ExpectedMethod.ToString() $actual.ToString() $Message
|
||||
}
|
||||
|
||||
Assert-Rank $cubicBSpline @(
|
||||
(New-Entry $cubicBSpline 1.0 0.5 0.8 5.0 10.0),
|
||||
(New-Entry $localCubicBezier 2.0 0.1 1.0 1.0 1.0)) 'Variation-energy tie-break must take priority over later criteria.'
|
||||
Assert-Rank $cubicBSpline @(
|
||||
(New-Entry $cubicBSpline 1.0 0.2 0.8 5.0 10.0),
|
||||
(New-Entry $localCubicBezier 1.0 0.3 1.0 1.0 1.0)) 'Peak-curvature tie-break must follow variation energy.'
|
||||
Assert-Rank $cubicBSpline @(
|
||||
(New-Entry $cubicBSpline 1.0 0.2 0.9 5.0 10.0),
|
||||
(New-Entry $localCubicBezier 1.0 0.2 0.8 1.0 1.0)) 'Clearance-loss tie-break must follow peak curvature.'
|
||||
Assert-Rank $cubicBSpline @(
|
||||
(New-Entry $cubicBSpline 1.0 0.2 0.9 2.0 10.0),
|
||||
(New-Entry $localCubicBezier 1.0 0.2 0.9 3.0 1.0)) 'Length-change tie-break must follow clearance loss.'
|
||||
Assert-Rank $cubicBSpline @(
|
||||
(New-Entry $cubicBSpline 1.0 0.2 0.9 2.0 5.0),
|
||||
(New-Entry $localCubicBezier 1.0 0.2 0.9 2.0 6.0)) 'Median elapsed tie-break must be last.'
|
||||
|
||||
$cancelSource = [Threading.CancellationTokenSource]::new()
|
||||
try {
|
||||
$cancelSource.Cancel()
|
||||
$cancelled = $compareMethod.Invoke($comparisonService, @($comparisonRequest, $cancelSource.Token))
|
||||
Assert-True $cancelled.IsCancelled 'Cancellation must stop comparison before subsequent methods start.'
|
||||
Assert-Equal $null $cancelled.RecommendedMethod 'Cancelled comparison must not make a recommendation.'
|
||||
}
|
||||
finally {
|
||||
$cancelSource.Dispose()
|
||||
}
|
||||
|
||||
Write-Output 'Path smoothing comparison checks passed.'
|
||||
@@ -0,0 +1,381 @@
|
||||
param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
|
||||
$ErrorActionPreference = 'Stop'
|
||||
|
||||
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
|
||||
$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
|
||||
$coarsePathRoot = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
|
||||
$mappingRoot = 'MultiWheelC.TrajectoryPlanning.Mapping.'
|
||||
|
||||
function Assert-True($Actual, [string]$Message) {
|
||||
if (-not $Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-Equal($Expected, $Actual, [string]$Message) {
|
||||
if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" }
|
||||
}
|
||||
|
||||
function Assert-Near([double]$Expected, [double]$Actual, [string]$Message) {
|
||||
if ([Math]::Abs($Expected - $Actual) -gt 0.000001d) {
|
||||
throw "$Message Expected=$Expected Actual=$Actual"
|
||||
}
|
||||
}
|
||||
|
||||
function Assert-Throws([scriptblock]$Action, [string]$Message) {
|
||||
$threw = $false
|
||||
try { & $Action }
|
||||
catch { $threw = $true }
|
||||
if (-not $threw) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-ReadOnlyCollection($Collection, [string]$Message) {
|
||||
$list = [System.Collections.IList]$Collection
|
||||
Assert-True ($null -ne $list) "$Message The collection must implement IList."
|
||||
Assert-True $list.IsReadOnly "$Message The collection must report IsReadOnly."
|
||||
Assert-Throws { $list.Add($null) } "$Message The collection must reject Add."
|
||||
}
|
||||
|
||||
function Get-RequiredType([string]$Name) {
|
||||
return $assembly.GetType($Name, $true)
|
||||
}
|
||||
|
||||
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
|
||||
$methodType = Get-RequiredType ($root + 'SmoothingMethod')
|
||||
$statusType = Get-RequiredType ($root + 'PathSmoothingStatus')
|
||||
$sourceType = Get-RequiredType ($root + 'SmoothedPathPointSource')
|
||||
$pointType = Get-RequiredType ($root + 'SmoothedPathPoint')
|
||||
$segmentType = Get-RequiredType ($root + 'SmoothedPathSegment')
|
||||
$bsplineOptionsType = Get-RequiredType ($root + 'CubicBSplineOptions')
|
||||
$bezierOptionsType = Get-RequiredType ($root + 'LocalCubicBezierOptions')
|
||||
$quinticOptionsType = Get-RequiredType ($root + 'PiecewiseQuinticOptions')
|
||||
$localOptionsType = Get-RequiredType ($root + 'LocalG2QuinticOptions')
|
||||
$regionStatusType = Get-RequiredType ($root + 'PathSmoothingRegionStatus')
|
||||
$regionFailureType = Get-RequiredType ($root + 'PathSmoothingRegionFailureReason')
|
||||
$regionReportType = Get-RequiredType ($root + 'PathSmoothingRegionReport')
|
||||
$requestType = Get-RequiredType ($root + 'PathSmoothingRequest')
|
||||
$metricsType = Get-RequiredType ($root + 'PathQualityMetrics')
|
||||
$diagnosticsType = Get-RequiredType ($root + 'PathSmoothingDiagnostics')
|
||||
$resultType = Get-RequiredType ($root + 'PathSmoothingResult')
|
||||
$directionType = Get-RequiredType ($coarsePathRoot + 'TravelDirection')
|
||||
$coarsePointType = Get-RequiredType ($coarsePathRoot + 'CoarsePathPoint')
|
||||
$coarseSegmentType = Get-RequiredType ($coarsePathRoot + 'PathSegment')
|
||||
$mapType = Get-RequiredType ($mappingRoot + 'PlanningGridMap')
|
||||
$vehicleType = Get-RequiredType ($coarsePathRoot + 'VehicleParameters')
|
||||
|
||||
Assert-Equal $true $methodType.IsEnum 'SmoothingMethod must be a public enum.'
|
||||
Assert-Equal $true $statusType.IsEnum 'PathSmoothingStatus must be a public enum.'
|
||||
Assert-Equal $true $sourceType.IsEnum 'SmoothedPathPointSource must be a public enum.'
|
||||
Assert-Equal 'CubicBSpline,LocalCubicBezier,PiecewiseQuintic,LocalG2Quintic' ([string]::Join(',', [Enum]::GetNames($methodType))) 'The Local G2 method must be appended without reordering legacy methods.'
|
||||
Assert-Equal 'Success,FallbackToCoarsePath,InvalidInput,Infeasible,Failed,Cancelled,Complete,PartialImprovement,NotNeeded,Unchanged' ([string]::Join(',', [Enum]::GetNames($statusType))) 'Local G2 statuses must be appended without reordering legacy statuses.'
|
||||
Assert-Equal 'Anchor,Interpolated,GearSwitch,CoarsePathFallback,LocalG2Transition' ([string]::Join(',', [Enum]::GetNames($sourceType))) 'Local G2 point source must be appended without reordering legacy sources.'
|
||||
Assert-Equal 'Improved,RetainedOriginal' ([string]::Join(',', [Enum]::GetNames($regionStatusType))) 'Local G2 region statuses must be stable.'
|
||||
Assert-Equal 'None,WindowUnavailable,CandidateGenerationFailed,Collision,InsufficientClearance,CurvatureExceeded,CurvatureOvershoot,DeviationExceeded,InsufficientImprovement,VariationCostRegression,GlobalValidationRollback' ([string]::Join(',', [Enum]::GetNames($regionFailureType))) 'Local G2 region failure reasons must be stable.'
|
||||
|
||||
$configuration = [Activator]::CreateInstance($configurationType)
|
||||
Assert-Near 0.025 $configuration.OutputSpacingMeters 'Default output spacing must be 0.025 m.'
|
||||
Assert-Near 0.025 $configuration.MaximumCollisionCheckStepMeters 'Default collision step must be 0.025 m.'
|
||||
Assert-Near 0.02 $configuration.MinimumClearanceReserveMeters 'Default clearance reserve must be 0.02 m.'
|
||||
Assert-Near 1.0 $configuration.SmoothingStrength 'Default smoothing strength must be 1.0.'
|
||||
Assert-Equal $true $configuration.AllowFallbackToCoarsePath 'Fallback must be enabled by default.'
|
||||
Assert-Equal 4 $configuration.RetryStrengthScales.Count 'Retry schedule must contain four entries.'
|
||||
Assert-Near 1.0 $configuration.RetryStrengthScales[0] 'First retry scale must be 1.0.'
|
||||
Assert-Near 0.75 $configuration.RetryStrengthScales[1] 'Second retry scale must be 0.75.'
|
||||
Assert-Near 0.50 $configuration.RetryStrengthScales[2] 'Third retry scale must be 0.50.'
|
||||
Assert-Near 0.25 $configuration.RetryStrengthScales[3] 'Last retry scale must be 0.25.'
|
||||
for ($index = 1; $index -lt $configuration.RetryStrengthScales.Count; $index++) {
|
||||
Assert-True ($configuration.RetryStrengthScales[$index] -lt $configuration.RetryStrengthScales[$index - 1]) 'Retry schedule must be strictly decreasing.'
|
||||
}
|
||||
Assert-ReadOnlyCollection $configuration.RetryStrengthScales 'Retry schedule must be immutable.'
|
||||
Assert-Near (1.0 / 3.0) ([Activator]::CreateInstance($bsplineOptionsType)).EndpointTangentScale 'B-spline endpoint tangent default must be one third.'
|
||||
$bezier = [Activator]::CreateInstance($bezierOptionsType)
|
||||
Assert-Near ([Math]::PI / 18.0) $bezier.CornerHeadingThresholdRadians 'Bezier corner threshold must be 10 degrees.'
|
||||
Assert-Near 0.60 $bezier.MaximumWindowLengthMeters 'Bezier window default must be 0.60 m.'
|
||||
Assert-Near (1.0 / 3.0) $bezier.HandleLengthRatio 'Bezier handle default must be one third.'
|
||||
$quintic = [Activator]::CreateInstance($quinticOptionsType)
|
||||
Assert-Near 0.50 $quintic.KnotSpacingMeters 'Quintic knot spacing must be 0.50 m.'
|
||||
Assert-Near 0.10 $quintic.MinimumKnotSpacingMeters 'Quintic minimum knot spacing must be 0.10 m.'
|
||||
$local = [Activator]::CreateInstance($localOptionsType)
|
||||
Assert-Near 0.20 $local.MinimumWindowLengthMeters 'Minimum Local G2 window must be 0.20 m.'
|
||||
Assert-Near 0.50 $local.PreferredWindowLengthMeters 'Preferred Local G2 window must be 0.50 m.'
|
||||
Assert-Near 0.80 $local.MaximumWindowLengthMeters 'Maximum Local G2 window must be 0.80 m.'
|
||||
Assert-Near 0.10 $local.MaximumDeviationMeters 'Maximum Local G2 deviation must be 0.10 m.'
|
||||
Assert-Near 0.001 $local.AbsoluteCurvatureJumpFloorPerMeter 'Absolute jump floor must be 0.001 1/m.'
|
||||
Assert-Near 0.05 $local.CurvatureJumpRatioOfMaximum 'Relative jump threshold must be 5 percent.'
|
||||
Assert-Near 0.20 $local.MinimumPeakGradientImprovementRatio 'Peak improvement must be 20 percent.'
|
||||
Assert-Near 0.02 $local.MaximumVariationCostRegressionRatio 'Variation cost tolerance must be 2 percent.'
|
||||
Assert-Equal 12 $local.MaximumCandidatesPerRegion 'At most twelve candidates are allowed.'
|
||||
|
||||
$forward = [Enum]::Parse($directionType, 'Forward')
|
||||
$anchor = [Enum]::Parse($sourceType, 'Anchor')
|
||||
$point = [Activator]::CreateInstance($pointType, @(
|
||||
[double]1.25, [double]-2.50, [double]0.30, [double]6.58, [double]4.75,
|
||||
$forward, [double]0.12, [double]0.12, [double]0.44, $false, $anchor))
|
||||
Assert-Near 1.25 $point.X 'Smoothed point X must be stored in m.'
|
||||
Assert-Near -2.50 $point.Y 'Smoothed point Y must be stored in m.'
|
||||
Assert-Near 0.30 $point.Heading 'Smoothed point heading must be stored in rad.'
|
||||
Assert-Near 6.58 $point.UnwrappedHeading 'Smoothed point unwrapped heading must be stored in rad.'
|
||||
Assert-Near 4.75 $point.ArcLength 'Smoothed point arc length must be stored in m.'
|
||||
Assert-Equal 'Forward' $point.Direction.ToString() 'Smoothed point direction must be preserved.'
|
||||
Assert-Near 0.12 $point.GeometricCurvature 'Smoothed point geometric curvature must be stored in 1/m.'
|
||||
Assert-Near 0.12 $point.VehicleCurvature 'Smoothed point vehicle curvature must be stored in 1/m.'
|
||||
Assert-Near 0.44 $point.BodyClearance 'Smoothed point clearance must be stored in m.'
|
||||
Assert-Equal $false $point.IsGearSwitchPoint 'Smoothed point gear-switch marker must be preserved.'
|
||||
Assert-Equal 'Anchor' $point.Source.ToString() 'Smoothed point source must be preserved.'
|
||||
Assert-True ($pointType.GetProperty('VehicleCurvatureDerivative') -ne $null) 'Smoothed points must expose d-kappa/d-s.'
|
||||
$pointWithDerivative = [Activator]::CreateInstance($pointType, @(
|
||||
[double]1.25, [double]-2.50, [double]0.30, [double]6.58, [double]4.75,
|
||||
$forward, [double]0.12, [double]0.12, [double]0.37, [double]0.44, $false, $anchor))
|
||||
Assert-Near 0.37 $pointWithDerivative.VehicleCurvatureDerivative 'Smoothed point curvature derivative must be stored in 1/m^2.'
|
||||
|
||||
$segmentA = [Activator]::CreateInstance($segmentType, @(0, $forward, 0, 2, $false, $true))
|
||||
$reverse = [Enum]::Parse($directionType, 'Reverse')
|
||||
$segmentB = [Activator]::CreateInstance($segmentType, @(1, $reverse, 3, 5, $true, $false))
|
||||
Assert-Equal 0 $segmentA.SegmentIndex 'First smoothing segment index must be retained.'
|
||||
Assert-Equal 'Forward' $segmentA.Direction.ToString() 'First smoothing segment direction must be retained.'
|
||||
Assert-Equal 2 $segmentA.EndIndex 'First smoothing segment end index must be retained.'
|
||||
Assert-Equal $true $segmentA.EndsAtGearSwitch 'First smoothing segment switch flag must be retained.'
|
||||
Assert-Equal 1 $segmentB.SegmentIndex 'Second smoothing segment index must be retained.'
|
||||
Assert-Equal 'Reverse' $segmentB.Direction.ToString() 'Second smoothing segment direction must be retained.'
|
||||
Assert-Equal $true $segmentB.StartsAtGearSwitch 'Second smoothing segment switch flag must be retained.'
|
||||
|
||||
$metrics = [Activator]::CreateInstance($metricsType)
|
||||
Assert-Equal $false $metrics.IsFeasible 'Default metrics must be infeasible until analysis accepts a candidate.'
|
||||
Assert-Near 0.0 $metrics.PathLengthMeters 'Default metrics must be zero-valued.'
|
||||
Assert-Near 0.0 $metrics.MinimumBodyClearanceMeters 'Default metrics must be zero-valued.'
|
||||
Assert-Near 0.0 $metrics.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter 'Default derivative metric must be zero-valued.'
|
||||
Assert-Near 0.0 $metrics.CurvatureVariationCost 'Curvature variation cost compatibility alias must be available.'
|
||||
$metricsWithDerivative = [Activator]::CreateInstance($metricsType, @(
|
||||
$true,
|
||||
[double]1.0, [double]0.50, [double]0.75, [double]0.0, [double]0.0,
|
||||
[double]0.0, [double]0.25, [double]0.0, [double]0.0, [double]0.0, [double]0.0))
|
||||
Assert-Near 0.75 $metricsWithDerivative.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter 'Derivative-aware metrics constructor must retain the peak derivative.'
|
||||
$diagnostics = [Activator]::CreateInstance($diagnosticsType)
|
||||
Assert-True ($diagnostics.Metrics -ne $null) 'Default diagnostics must provide quality metrics.'
|
||||
Assert-Equal 0 $diagnostics.RetryCount 'Default diagnostics must have no retries.'
|
||||
Assert-Near 0.0 $diagnostics.AcceptedStrength 'Default diagnostics must have zero accepted strength.'
|
||||
$feasibleMetrics = [Activator]::CreateInstance($metricsType, @(
|
||||
$true,
|
||||
[double]1.0, [double]0.0, [double]0.0, [double]0.0, [double]0.0,
|
||||
[double]0.5, [double]0.0, [double]0.0, [double]0.0, [double]0.0))
|
||||
$feasibleDiagnostics = [Activator]::CreateInstance($diagnosticsType, @(
|
||||
$feasibleMetrics, [TimeSpan]::Zero, 0, [double]1.0, 'test feasible diagnostics'))
|
||||
|
||||
$pointArray = [Array]::CreateInstance($pointType, 1)
|
||||
$pointArray.SetValue($point, 0)
|
||||
$segmentArray = [Array]::CreateInstance($segmentType, 2)
|
||||
$segmentArray.SetValue($segmentA, 0)
|
||||
$segmentArray.SetValue($segmentB, 1)
|
||||
$method = [Enum]::Parse($methodType, 'CubicBSpline')
|
||||
$successMethod = $resultType.GetMethod('Success')
|
||||
Assert-True ($null -ne $successMethod) 'PathSmoothingResult must expose Success.'
|
||||
Assert-Throws { $successMethod.Invoke($null, @($method, $pointArray, $segmentArray, $diagnostics)) } 'Success factory must reject diagnostics that are not feasible.'
|
||||
Assert-Throws { $successMethod.Invoke($null, @($method, $pointArray, $segmentArray, $null)) } 'Success factory must reject null diagnostics.'
|
||||
Assert-Throws { $successMethod.Invoke($null, @([Enum]::ToObject($methodType, 99), $pointArray, $segmentArray, $feasibleDiagnostics)) } 'Success factory must reject undefined smoothing methods.'
|
||||
$success = $successMethod.Invoke($null, @($method, $pointArray, $segmentArray, $feasibleDiagnostics))
|
||||
Assert-Equal 'Success' $success.Status.ToString() 'Success factory must publish Success status.'
|
||||
Assert-Equal 'CubicBSpline' $success.Method.ToString() 'Success factory must retain the selected method.'
|
||||
Assert-Equal 1 $success.Path.Count 'Success factory must publish the provided path.'
|
||||
Assert-Equal 2 $success.Segments.Count 'Success factory must publish the provided segments.'
|
||||
Assert-ReadOnlyCollection $success.Path 'Success path must be immutable.'
|
||||
Assert-ReadOnlyCollection $success.Segments 'Success segments must be immutable.'
|
||||
$pointArray.SetValue($null, 0)
|
||||
$segmentArray.SetValue($null, 0)
|
||||
Assert-True ($null -ne $success.Path[0]) 'Success factory must copy path collections.'
|
||||
Assert-True ($null -ne $success.Segments[0]) 'Success factory must copy segment collections.'
|
||||
|
||||
$fallbackMethod = $resultType.GetMethod('Fallback')
|
||||
Assert-True ($null -ne $fallbackMethod) 'PathSmoothingResult must expose Fallback.'
|
||||
$fallbackPath = [Array]::CreateInstance($pointType, 1)
|
||||
$fallbackPath.SetValue($point, 0)
|
||||
$fallbackSegments = [Array]::CreateInstance($segmentType, 1)
|
||||
$fallbackSegments.SetValue($segmentA, 0)
|
||||
Assert-Throws { $fallbackMethod.Invoke($null, @($method, $fallbackPath, $fallbackSegments, $diagnostics)) } 'Fallback factory must reject diagnostics that are not feasible.'
|
||||
Assert-Throws { $fallbackMethod.Invoke($null, @($method, $fallbackPath, $fallbackSegments, $null)) } 'Fallback factory must reject null diagnostics.'
|
||||
Assert-Throws { $fallbackMethod.Invoke($null, @([Enum]::ToObject($methodType, 99), $fallbackPath, $fallbackSegments, $feasibleDiagnostics)) } 'Fallback factory must reject undefined smoothing methods.'
|
||||
$fallback = $fallbackMethod.Invoke($null, @($method, $fallbackPath, $fallbackSegments, $feasibleDiagnostics))
|
||||
Assert-Equal 'FallbackToCoarsePath' $fallback.Status.ToString() 'Fallback factory must publish an explicit fallback status.'
|
||||
Assert-Equal 1 $fallback.Path.Count 'Fallback factory must publish a validated fallback path.'
|
||||
Assert-Equal 0 $fallback.RegionReports.Count 'Legacy fallback results must publish empty immutable region reports.'
|
||||
|
||||
$failureMethod = $resultType.GetMethod('Failure')
|
||||
Assert-True ($null -ne $failureMethod) 'PathSmoothingResult must expose Failure.'
|
||||
$failed = $failureMethod.Invoke(
|
||||
$null,
|
||||
@([Enum]::Parse($statusType, 'InvalidInput'),
|
||||
[Activator]::CreateInstance($diagnosticsType)))
|
||||
Assert-Equal 'InvalidInput' $failed.Status.ToString() 'Failure factory must retain failure status.'
|
||||
Assert-Equal 0 $failed.Path.Count 'Failure must publish no path.'
|
||||
Assert-Equal 0 $failed.Segments.Count 'Failure must publish no segments.'
|
||||
Assert-ReadOnlyCollection $failed.Path 'Failure path must be immutable.'
|
||||
Assert-ReadOnlyCollection $failed.Segments 'Failure segments must be immutable.'
|
||||
Assert-Throws { $failureMethod.Invoke($null, @([Enum]::Parse($statusType, 'Success'), $diagnostics)) } 'Failure factory must reject Success.'
|
||||
Assert-Throws { $failureMethod.Invoke($null, @([Enum]::Parse($statusType, 'FallbackToCoarsePath'), $diagnostics)) } 'Failure factory must reject fallback status.'
|
||||
Assert-Throws { $failureMethod.Invoke($null, @([Enum]::ToObject($statusType, 99), $diagnostics)) } 'Failure factory must reject undefined statuses.'
|
||||
Assert-Throws { $successMethod.Invoke($null, @($method, [Array]::CreateInstance($pointType, 0), $fallbackSegments, $diagnostics)) } 'Success factory must reject an empty path.'
|
||||
Assert-Throws { $successMethod.Invoke($null, @($method, $fallbackPath, [Array]::CreateInstance($segmentType, 0), $diagnostics)) } 'Success factory must reject empty segments.'
|
||||
Assert-Equal 0 $success.RegionReports.Count 'Legacy success results must publish empty immutable region reports.'
|
||||
Assert-ReadOnlyCollection $success.RegionReports 'Legacy success region reports must be immutable.'
|
||||
|
||||
$curvatureJumps = [System.Collections.Generic.List[double]]::new()
|
||||
$curvatureJumps.Add([double]0.20)
|
||||
$report = [Activator]::CreateInstance($regionReportType, @(
|
||||
0, [double]0.0, [double]0.5, $curvatureJumps,
|
||||
[double]0.5, [double]0.5, [double]0.25, [double]0.25,
|
||||
1, 0,
|
||||
[Enum]::Parse($regionStatusType, 'Improved'), [Enum]::Parse($regionFailureType, 'None'),
|
||||
[double]1.0, [double]0.5, [double]2.0, [double]1.0,
|
||||
[double]0.05, [double]0.10, [double]0.80))
|
||||
Assert-ReadOnlyCollection $report.CurvatureJumpsPerMeter 'Region report curvature jumps must be immutable.'
|
||||
$curvatureJumps[0] = [double]9.99
|
||||
Assert-Near 0.20 $report.CurvatureJumpsPerMeter[0] 'Region report must copy curvature jumps.'
|
||||
$retainedReport = [Activator]::CreateInstance($regionReportType, @(
|
||||
0, [double]0.0, [double]0.5, $curvatureJumps,
|
||||
[double]0.5, [double]0.5, [double]0.25, [double]0.25,
|
||||
1, 7,
|
||||
[Enum]::Parse($regionStatusType, 'RetainedOriginal'), [Enum]::Parse($regionFailureType, 'InsufficientImprovement'),
|
||||
[double]1.0, [double]1.0, [double]2.0, [double]2.0,
|
||||
[double]0.0, [double]0.10, [double]0.80))
|
||||
Assert-Equal -1 $retainedReport.SelectedCandidateIndex 'A region without a selected candidate must publish -1.'
|
||||
$publishLocalG2Method = $resultType.GetMethod('PublishLocalG2')
|
||||
Assert-True ($null -ne $publishLocalG2Method) 'PathSmoothingResult must expose PublishLocalG2.'
|
||||
$reports = [Array]::CreateInstance($regionReportType, 1)
|
||||
$reports.SetValue($report, 0)
|
||||
$localG2Method = [Enum]::Parse($methodType, 'LocalG2Quintic')
|
||||
$complete = [Enum]::Parse($statusType, 'Complete')
|
||||
$localG2Result = $publishLocalG2Method.Invoke($null, @($complete, $fallbackPath, $fallbackSegments, $feasibleDiagnostics, $reports))
|
||||
Assert-Equal 'Complete' $localG2Result.Status.ToString() 'PublishLocalG2 must retain Local G2 publication status.'
|
||||
Assert-Equal 'LocalG2Quintic' $localG2Result.Method.ToString() 'PublishLocalG2 must publish the Local G2 method.'
|
||||
Assert-Equal 1 $localG2Result.RegionReports.Count 'PublishLocalG2 must publish region reports.'
|
||||
Assert-ReadOnlyCollection $localG2Result.RegionReports 'Local G2 result region reports must be immutable.'
|
||||
$reports.SetValue($null, 0)
|
||||
Assert-True ($null -ne $localG2Result.RegionReports[0]) 'PublishLocalG2 must copy region reports.'
|
||||
Assert-Throws { $publishLocalG2Method.Invoke($null, @([Enum]::Parse($statusType, 'Success'), $fallbackPath, $fallbackSegments, $feasibleDiagnostics, $reports)) } 'PublishLocalG2 must reject legacy statuses.'
|
||||
Assert-Throws { $publishLocalG2Method.Invoke($null, @($complete, [Array]::CreateInstance($pointType, 0), $fallbackSegments, $feasibleDiagnostics, $reports)) } 'PublishLocalG2 must reject an empty path.'
|
||||
|
||||
$requestConstructor = $requestType.GetConstructor(@(
|
||||
[System.Collections.Generic.IReadOnlyList``1].MakeGenericType($coarsePointType),
|
||||
[System.Collections.Generic.IReadOnlyList``1].MakeGenericType($coarseSegmentType),
|
||||
$mapType,
|
||||
$vehicleType,
|
||||
$configurationType))
|
||||
Assert-True ($null -ne $requestConstructor) 'PathSmoothingRequest must expose the public five-argument constructor.'
|
||||
$boundsType = Get-RequiredType ($mappingRoot + 'MapBoundsMm')
|
||||
$mapRequestType = Get-RequiredType ($mappingRoot + 'PlanningMapRequest')
|
||||
$mapFactoryType = Get-RequiredType ($mappingRoot + 'PlanningMapFactory')
|
||||
$mapRequest = [Activator]::CreateInstance($mapRequestType)
|
||||
$mapRequest.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]1000, [single]0, [single]1000))
|
||||
$mapRequest.ResolutionMm = [single]50
|
||||
$mapRequest.AllowExplicitEmptyMap = $true
|
||||
$map = [Activator]::CreateInstance($mapFactoryType).Create($mapRequest).Map
|
||||
Assert-True ($null -ne $map) 'Contract test must create an explicit empty planning map.'
|
||||
$requestCoarsePath = [Array]::CreateInstance($coarsePointType, 1)
|
||||
$requestCoarsePath.SetValue([Activator]::CreateInstance($coarsePointType, @(
|
||||
[double]0.0, [double]0.0, [double]0.0, [double]0.0, [double]0.0,
|
||||
$forward, [double]0.0, [double]1.0, $false,
|
||||
[Enum]::Parse((Get-RequiredType ($coarsePathRoot + 'CoarsePathPointSource')), 'Start'))), 0)
|
||||
$requestSegments = [Array]::CreateInstance($coarseSegmentType, 1)
|
||||
$requestSegments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(0, $forward, 0, 0, $false, $false)), 0)
|
||||
$vehicle = [Activator]::CreateInstance($vehicleType)
|
||||
$vehicle.LengthMeters = [double]0.80
|
||||
$vehicle.WidthMeters = [double]0.60
|
||||
$vehicle.SafetyMarginMeters = [double]0.05
|
||||
$vehicle.MaximumCurvaturePerMeter = [double]0.8333333333333334
|
||||
$requestConfiguration = [Activator]::CreateInstance($configurationType)
|
||||
$request = $requestConstructor.Invoke(@($requestCoarsePath, $requestSegments, $map, $vehicle, $requestConfiguration))
|
||||
Assert-ReadOnlyCollection $request.CoarsePath 'Request coarse path must be immutable.'
|
||||
Assert-ReadOnlyCollection $request.Segments 'Request segments must be immutable.'
|
||||
$requestCoarsePath.SetValue($null, 0)
|
||||
$requestSegments.SetValue($null, 0)
|
||||
$vehicle.LengthMeters = [double]9.99
|
||||
$vehicle.WidthMeters = [double]9.99
|
||||
$vehicle.SafetyMarginMeters = [double]9.99
|
||||
$vehicle.MaximumCurvaturePerMeter = [double]0.1
|
||||
$vehicle.MinimumTurningRadiusMeters = [double]9.99
|
||||
$requestConfiguration.Method = [Enum]::Parse($methodType, 'PiecewiseQuintic')
|
||||
$requestConfiguration.OutputSpacingMeters = [double]0.99
|
||||
$requestConfiguration.MaximumCollisionCheckStepMeters = [double]0.99
|
||||
$requestConfiguration.MinimumClearanceReserveMeters = [double]0.99
|
||||
$requestConfiguration.SmoothingStrength = [double]0.99
|
||||
$requestConfiguration.AllowFallbackToCoarsePath = $false
|
||||
$requestConfiguration.CubicBSpline.EndpointTangentScale = [double]0.99
|
||||
$requestConfiguration.LocalCubicBezier.CornerHeadingThresholdRadians = [double]0.99
|
||||
$requestConfiguration.LocalCubicBezier.MaximumWindowLengthMeters = [double]0.99
|
||||
$requestConfiguration.LocalCubicBezier.HandleLengthRatio = [double]0.99
|
||||
$requestConfiguration.PiecewiseQuintic.KnotSpacingMeters = [double]0.99
|
||||
$requestConfiguration.PiecewiseQuintic.MinimumKnotSpacingMeters = [double]0.99
|
||||
$requestConfiguration.LocalG2Quintic.MinimumWindowLengthMeters = [double]0.99
|
||||
$requestConfiguration.LocalG2Quintic.PreferredWindowLengthMeters = [double]0.99
|
||||
$requestConfiguration.LocalG2Quintic.MaximumWindowLengthMeters = [double]0.99
|
||||
$requestConfiguration.LocalG2Quintic.MaximumDeviationMeters = [double]0.99
|
||||
$requestConfiguration.LocalG2Quintic.AbsoluteCurvatureJumpFloorPerMeter = [double]0.99
|
||||
$requestConfiguration.LocalG2Quintic.CurvatureJumpRatioOfMaximum = [double]0.99
|
||||
$requestConfiguration.LocalG2Quintic.MinimumPeakGradientImprovementRatio = [double]0.99
|
||||
$requestConfiguration.LocalG2Quintic.MaximumVariationCostRegressionRatio = [double]0.99
|
||||
$requestConfiguration.LocalG2Quintic.MaximumCandidatesPerRegion = 99
|
||||
Assert-True ($null -ne $request.CoarsePath[0]) 'Request must copy the coarse-path collection.'
|
||||
Assert-True ($null -ne $request.Segments[0]) 'Request must copy the segment collection.'
|
||||
Assert-Near 0.80 $request.Vehicle.LengthMeters 'Request must snapshot vehicle parameters.'
|
||||
Assert-Near 0.60 $request.Vehicle.WidthMeters 'Request must snapshot vehicle width.'
|
||||
Assert-Near 0.05 $request.Vehicle.SafetyMarginMeters 'Request must snapshot vehicle safety margin.'
|
||||
Assert-Near (1.0 / 1.20) $request.Vehicle.MaximumCurvaturePerMeter 'Request must snapshot nullable vehicle curvature.'
|
||||
Assert-True ($null -eq $request.Vehicle.MinimumTurningRadiusMeters) 'Request must snapshot nullable vehicle turning radius.'
|
||||
Assert-Equal 'CubicBSpline' $request.Configuration.Method.ToString() 'Request must snapshot smoothing method.'
|
||||
Assert-Near 0.025 $request.Configuration.OutputSpacingMeters 'Request must snapshot common configuration.'
|
||||
Assert-Near 0.025 $request.Configuration.MaximumCollisionCheckStepMeters 'Request must snapshot collision configuration.'
|
||||
Assert-Near 0.02 $request.Configuration.MinimumClearanceReserveMeters 'Request must snapshot clearance configuration.'
|
||||
Assert-Near 1.0 $request.Configuration.SmoothingStrength 'Request must snapshot smoothing strength.'
|
||||
Assert-Equal $true $request.Configuration.AllowFallbackToCoarsePath 'Request must snapshot fallback configuration.'
|
||||
Assert-Near (1.0 / 3.0) $request.Configuration.CubicBSpline.EndpointTangentScale 'Request must snapshot B-spline options.'
|
||||
Assert-Near ([Math]::PI / 18.0) $request.Configuration.LocalCubicBezier.CornerHeadingThresholdRadians 'Request must snapshot Bezier threshold.'
|
||||
Assert-Near 0.60 $request.Configuration.LocalCubicBezier.MaximumWindowLengthMeters 'Request must snapshot Bezier window length.'
|
||||
Assert-Near (1.0 / 3.0) $request.Configuration.LocalCubicBezier.HandleLengthRatio 'Request must snapshot Bezier options.'
|
||||
Assert-Near 0.50 $request.Configuration.PiecewiseQuintic.KnotSpacingMeters 'Request must snapshot quintic options.'
|
||||
Assert-Near 0.10 $request.Configuration.PiecewiseQuintic.MinimumKnotSpacingMeters 'Request must snapshot quintic minimum spacing.'
|
||||
Assert-Near 0.20 $request.Configuration.LocalG2Quintic.MinimumWindowLengthMeters 'Request must snapshot Local G2 minimum window.'
|
||||
Assert-Near 0.50 $request.Configuration.LocalG2Quintic.PreferredWindowLengthMeters 'Request must snapshot Local G2 preferred window.'
|
||||
Assert-Near 0.80 $request.Configuration.LocalG2Quintic.MaximumWindowLengthMeters 'Request must snapshot Local G2 maximum window.'
|
||||
Assert-Near 0.10 $request.Configuration.LocalG2Quintic.MaximumDeviationMeters 'Request must snapshot Local G2 maximum deviation.'
|
||||
Assert-Near 0.001 $request.Configuration.LocalG2Quintic.AbsoluteCurvatureJumpFloorPerMeter 'Request must snapshot Local G2 absolute jump floor.'
|
||||
Assert-Near 0.05 $request.Configuration.LocalG2Quintic.CurvatureJumpRatioOfMaximum 'Request must snapshot Local G2 relative jump threshold.'
|
||||
Assert-Near 0.20 $request.Configuration.LocalG2Quintic.MinimumPeakGradientImprovementRatio 'Request must snapshot Local G2 peak improvement threshold.'
|
||||
Assert-Near 0.02 $request.Configuration.LocalG2Quintic.MaximumVariationCostRegressionRatio 'Request must snapshot Local G2 variation tolerance.'
|
||||
Assert-Equal 12 $request.Configuration.LocalG2Quintic.MaximumCandidatesPerRegion 'Request must snapshot Local G2 candidate count.'
|
||||
$request.Vehicle.WidthMeters = [double]9.99
|
||||
$request.Vehicle.SafetyMarginMeters = [double]9.99
|
||||
$request.Vehicle.MinimumTurningRadiusMeters = [double]9.99
|
||||
$request.Configuration.MaximumCollisionCheckStepMeters = [double]0.99
|
||||
$request.Configuration.MinimumClearanceReserveMeters = [double]0.99
|
||||
$request.Configuration.SmoothingStrength = [double]0.99
|
||||
$request.Configuration.AllowFallbackToCoarsePath = $false
|
||||
$request.Configuration.LocalCubicBezier.CornerHeadingThresholdRadians = [double]0.99
|
||||
$request.Configuration.LocalCubicBezier.MaximumWindowLengthMeters = [double]0.99
|
||||
$request.Configuration.PiecewiseQuintic.MinimumKnotSpacingMeters = [double]0.99
|
||||
$request.Configuration.LocalG2Quintic.MinimumWindowLengthMeters = [double]0.99
|
||||
$request.Configuration.LocalG2Quintic.PreferredWindowLengthMeters = [double]0.99
|
||||
$request.Configuration.LocalG2Quintic.MaximumWindowLengthMeters = [double]0.99
|
||||
$request.Configuration.LocalG2Quintic.MaximumDeviationMeters = [double]0.99
|
||||
$request.Configuration.LocalG2Quintic.AbsoluteCurvatureJumpFloorPerMeter = [double]0.99
|
||||
$request.Configuration.LocalG2Quintic.CurvatureJumpRatioOfMaximum = [double]0.99
|
||||
$request.Configuration.LocalG2Quintic.MinimumPeakGradientImprovementRatio = [double]0.99
|
||||
$request.Configuration.LocalG2Quintic.MaximumVariationCostRegressionRatio = [double]0.99
|
||||
$request.Configuration.LocalG2Quintic.MaximumCandidatesPerRegion = 99
|
||||
Assert-Near 0.60 $request.Vehicle.WidthMeters 'Request vehicle getter must not expose mutable state.'
|
||||
Assert-Near 0.05 $request.Vehicle.SafetyMarginMeters 'Request vehicle getter must not expose mutable state.'
|
||||
Assert-True ($null -eq $request.Vehicle.MinimumTurningRadiusMeters) 'Request vehicle getter must not expose mutable nullable state.'
|
||||
Assert-Near 0.025 $request.Configuration.MaximumCollisionCheckStepMeters 'Request configuration getter must not expose mutable state.'
|
||||
Assert-Near 0.02 $request.Configuration.MinimumClearanceReserveMeters 'Request configuration getter must not expose mutable state.'
|
||||
Assert-Near 1.0 $request.Configuration.SmoothingStrength 'Request configuration getter must not expose mutable state.'
|
||||
Assert-Equal $true $request.Configuration.AllowFallbackToCoarsePath 'Request configuration getter must not expose mutable state.'
|
||||
Assert-Near ([Math]::PI / 18.0) $request.Configuration.LocalCubicBezier.CornerHeadingThresholdRadians 'Request configuration getter must not expose mutable Bezier options.'
|
||||
Assert-Near 0.60 $request.Configuration.LocalCubicBezier.MaximumWindowLengthMeters 'Request configuration getter must not expose mutable Bezier options.'
|
||||
Assert-Near 0.10 $request.Configuration.PiecewiseQuintic.MinimumKnotSpacingMeters 'Request configuration getter must not expose mutable quintic options.'
|
||||
Assert-Near 0.20 $request.Configuration.LocalG2Quintic.MinimumWindowLengthMeters 'Request configuration getter must not expose mutable Local G2 minimum window.'
|
||||
Assert-Near 0.50 $request.Configuration.LocalG2Quintic.PreferredWindowLengthMeters 'Request configuration getter must not expose mutable Local G2 preferred window.'
|
||||
Assert-Near 0.80 $request.Configuration.LocalG2Quintic.MaximumWindowLengthMeters 'Request configuration getter must not expose mutable Local G2 maximum window.'
|
||||
Assert-Near 0.10 $request.Configuration.LocalG2Quintic.MaximumDeviationMeters 'Request configuration getter must not expose mutable Local G2 maximum deviation.'
|
||||
Assert-Near 0.001 $request.Configuration.LocalG2Quintic.AbsoluteCurvatureJumpFloorPerMeter 'Request configuration getter must not expose mutable Local G2 absolute jump floor.'
|
||||
Assert-Near 0.05 $request.Configuration.LocalG2Quintic.CurvatureJumpRatioOfMaximum 'Request configuration getter must not expose mutable Local G2 relative jump threshold.'
|
||||
Assert-Near 0.20 $request.Configuration.LocalG2Quintic.MinimumPeakGradientImprovementRatio 'Request configuration getter must not expose mutable Local G2 peak improvement threshold.'
|
||||
Assert-Near 0.02 $request.Configuration.LocalG2Quintic.MaximumVariationCostRegressionRatio 'Request configuration getter must not expose mutable Local G2 variation tolerance.'
|
||||
Assert-Equal 12 $request.Configuration.LocalG2Quintic.MaximumCandidatesPerRegion 'Request configuration getter must not expose mutable Local G2 candidate count.'
|
||||
|
||||
Write-Output 'Path smoothing contract checks passed.'
|
||||
@@ -0,0 +1,65 @@
|
||||
$ErrorActionPreference = 'Stop'
|
||||
|
||||
$root = Join-Path $PSScriptRoot '..'
|
||||
$readmePath = Join-Path $root 'ParkrobTrajplanner\PathSmoothing\README.md'
|
||||
$demoPath = Join-Path $root 'ParkrobTrajplanner\PathSmoothing\Test\PathSmoothingComparisonDemo.cs'
|
||||
$runnerPath = Join-Path $PSScriptRoot 'run_path_smoothing_comparison.ps1'
|
||||
|
||||
function Assert-True($actual, [string]$message) {
|
||||
if (-not $actual) { throw $message }
|
||||
}
|
||||
|
||||
Assert-True (Test-Path -LiteralPath $readmePath) 'Path smoothing README is missing.'
|
||||
Assert-True (Test-Path -LiteralPath $demoPath) 'Path smoothing comparison demo is missing.'
|
||||
Assert-True (Test-Path -LiteralPath $runnerPath) 'Path smoothing comparison batch runner is missing.'
|
||||
|
||||
$readme = Get-Content -Raw -Encoding UTF8 $readmePath
|
||||
foreach ($section in @(
|
||||
'## Units and coordinates',
|
||||
'## Facade usage',
|
||||
'## Status handling and fallback',
|
||||
'## Fixture freshness',
|
||||
'## IEEE colors, fonts, and Windows PNG',
|
||||
'## SQP boundary',
|
||||
'## Output files')) {
|
||||
Assert-True $readme.Contains($section) "README must contain section: $section"
|
||||
}
|
||||
foreach ($requiredText in @(
|
||||
'meters',
|
||||
'radians',
|
||||
'PathSmoothingService',
|
||||
'FallbackToCoarsePath',
|
||||
'SimSun',
|
||||
'Times New Roman',
|
||||
'600 dpi',
|
||||
'discrete samples',
|
||||
'01-coarse-path-overview',
|
||||
'02-all-paths-comparison',
|
||||
'03-cubic-bspline-overview',
|
||||
'04-local-cubic-bezier-overview',
|
||||
'05-piecewise-quintic-overview',
|
||||
'06-curvature-comparison',
|
||||
'PDF/EPS')) {
|
||||
Assert-True $readme.Contains($requiredText) "README must document: $requiredText"
|
||||
}
|
||||
|
||||
$demo = Get-Content -Raw -Encoding UTF8 $demoPath
|
||||
foreach ($requiredText in @(
|
||||
'PathSmoothingComparisonDemo',
|
||||
'PathSmoothingService',
|
||||
'PathSmoothingComparisonService',
|
||||
'SmoothingReportExporter',
|
||||
'SmoothingScenarioFactory',
|
||||
'PathSmoothingStatus.Success',
|
||||
'PathSmoothingStatus.FallbackToCoarsePath')) {
|
||||
Assert-True $demo.Contains($requiredText) "Demo must use: $requiredText"
|
||||
}
|
||||
|
||||
$runner = Get-Content -Raw -Encoding UTF8 $runnerPath
|
||||
Assert-True ($runner -match '\[switch\]\$FixtureOnly') 'Batch runner must offer -FixtureOnly.'
|
||||
Assert-True $runner.Contains('obj\path_smoothing_reports') 'Batch runner must write reports below ClumsyPilot/obj/path_smoothing_reports.'
|
||||
Assert-True $runner.Contains('--export-fixtures') 'Batch runner must export all eight fast fixtures by default.'
|
||||
Assert-True $runner.Contains('if (-not $FixtureOnly)') 'Batch runner must make end-to-end cases optional.'
|
||||
Assert-True ($runner -notmatch 'ParkrobTrajplanner\\PathSmoothing\\.*\.(svg|png|csv)') 'Batch runner must never write report files below source directories.'
|
||||
|
||||
Write-Output 'Path smoothing documentation checks passed.'
|
||||
@@ -0,0 +1,119 @@
|
||||
param(
|
||||
[string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'),
|
||||
[string]$FixturePath = (Join-Path $PSScriptRoot '..\ParkrobTrajplanner\PathSmoothing\Test\Fixtures\path-smoothing-fixtures.json'))
|
||||
|
||||
$ErrorActionPreference = 'Stop'
|
||||
$newtonsoftPath = Join-Path $env:USERPROFILE '.nuget\packages\newtonsoft.json\13.0.4\lib\netstandard2.0\Newtonsoft.Json.dll'
|
||||
if (Test-Path -LiteralPath $newtonsoftPath) { [Reflection.Assembly]::LoadFrom($newtonsoftPath) | Out-Null }
|
||||
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
|
||||
|
||||
function Assert-True($Actual, [string]$Message) { if (-not $Actual) { throw $Message } }
|
||||
function Assert-Equal($Expected, $Actual, [string]$Message) {
|
||||
if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" }
|
||||
}
|
||||
function Assert-ThrowsMatching([scriptblock]$Action, [string]$ExpectedPattern, [string]$Message) {
|
||||
try {
|
||||
& $Action
|
||||
}
|
||||
catch {
|
||||
$exception = $_.Exception
|
||||
while ($null -ne $exception.InnerException) { $exception = $exception.InnerException }
|
||||
if ($exception.Message -match $ExpectedPattern) { return }
|
||||
throw "$Message ExpectedPattern=$ExpectedPattern Actual=$($exception.Message)"
|
||||
}
|
||||
throw "$Message Expected an exception."
|
||||
}
|
||||
function Get-RequiredType([string]$Name) { return $assembly.GetType($Name, $true) }
|
||||
|
||||
$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.Test.'
|
||||
$comparison = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison.'
|
||||
$loaderType = Get-RequiredType ($root + 'SmoothingScenarioFixtureLoader')
|
||||
$factoryType = Get-RequiredType ($root + 'SmoothingScenarioFactory')
|
||||
$fixtureType = Get-RequiredType ($root + 'SmoothingScenarioFixture')
|
||||
$comparisonRequestType = Get-RequiredType ($comparison + 'PathSmoothingComparisonRequest')
|
||||
$preprocessorType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing.PathSmoothingPreprocessor'
|
||||
|
||||
$loadMethod = $loaderType.GetMethod('LoadAndVerify', [Type[]]@([string]))
|
||||
Assert-True ($null -ne $loadMethod) 'Fixture loader must expose LoadAndVerify(string).'
|
||||
$fixtureRequestsMethod = $factoryType.GetMethod('CreateFixtureRequests', [Type[]]@([string]))
|
||||
Assert-True ($null -ne $fixtureRequestsMethod) 'Scenario factory must expose CreateFixtureRequests(string).'
|
||||
$generateMethod = $assembly.GetType($root + 'SmoothingFixtureGenerator', $true).GetMethod('Generate', [Type[]]@([string], [bool]))
|
||||
Assert-True ($null -ne $generateMethod) 'Fixture generator must expose Generate(string, bool).'
|
||||
$tryPrepareMethod = $preprocessorType.GetMethod('TryPrepare')
|
||||
Assert-True ($null -ne $tryPrepareMethod) 'Fixture paths must be checked through PathSmoothingPreprocessor.TryPrepare.'
|
||||
|
||||
$fixtures = $loadMethod.Invoke($null, @((Resolve-Path $FixturePath).Path))
|
||||
$fixtureDocument = Get-Content -Raw -Encoding UTF8 $FixturePath | ConvertFrom-Json
|
||||
$expected = @(
|
||||
'straight', 'single-turn', 's-bend', 'large-heading-change',
|
||||
'rectangle-detour', 'multi-obstacle-detour',
|
||||
'narrow-corridor', 'forward-reverse-switch')
|
||||
Assert-Equal $expected.Count $fixtures.Count 'Fixture loader must return exactly eight fast fixtures.'
|
||||
|
||||
$actualIds = @($fixtures | ForEach-Object { $_.Id })
|
||||
Assert-Equal ($expected -join ',') ($actualIds -join ',') 'Fixture IDs must be stable and in documented order.'
|
||||
Assert-Equal $actualIds.Count (@($actualIds | Select-Object -Unique).Count) 'Fixture IDs must be unique.'
|
||||
foreach ($fixture in $fixtures) {
|
||||
Assert-True ($fixture -is $fixtureType) 'Loader must return immutable smoothing fixture values.'
|
||||
Assert-True ($fixture.FixtureVersion -gt 0) "Fixture $($fixture.Id) must carry a positive version."
|
||||
Assert-True $fixture.IsConfigurationFingerprintCurrent "Fixture $($fixture.Id) must match its stored configuration fingerprint."
|
||||
Assert-True ($fixture.ConfigurationFingerprint -match '^sha256:[0-9a-f]{64}$') "Fixture $($fixture.Id) must expose a lowercase SHA-256 fingerprint."
|
||||
Assert-True ($fixture.Path.Count -gt 1) "Fixture $($fixture.Id) must include a coarse path."
|
||||
Assert-True ($fixture.Segments.Count -gt 0) "Fixture $($fixture.Id) must include direction segments."
|
||||
foreach ($segment in $fixture.Segments) {
|
||||
Assert-True ($segment.StartIndex -ge 0 -and $segment.EndIndex -lt $fixture.Path.Count -and $segment.EndIndex -ge $segment.StartIndex) "Fixture $($fixture.Id) must retain valid segment coverage."
|
||||
}
|
||||
}
|
||||
foreach ($record in $fixtureDocument.scenarios) {
|
||||
Assert-True ($null -ne $record.planningConfiguration) "Fixture $($record.id) must retain the planning configuration that produced its coarse path."
|
||||
}
|
||||
|
||||
$requests = $fixtureRequestsMethod.Invoke($null, @((Resolve-Path $FixturePath).Path))
|
||||
Assert-Equal 8 $requests.Count 'Fast fixtures must create exactly eight comparison requests without Hybrid A*.'
|
||||
foreach ($request in $requests) {
|
||||
Assert-True ($request -is $comparisonRequestType) 'Fast fixture factory must create comparison requests.'
|
||||
$prepareArguments = [object[]]@($request.SmoothingRequest, $null, $null)
|
||||
$prepared = $tryPrepareMethod.Invoke([Activator]::CreateInstance($preprocessorType), $prepareArguments)
|
||||
Assert-True $prepared "Fixture path must satisfy the PathSmoothingPreprocessor input contract. Reason=$($prepareArguments[2])"
|
||||
}
|
||||
|
||||
$loaderSource = Get-Content -Raw -Encoding UTF8 (Join-Path $PSScriptRoot '..\ParkrobTrajplanner\PathSmoothing\Test\SmoothingScenarioFixtureLoader.cs')
|
||||
Assert-True (-not $loaderSource.Contains('HybridAStarPlanner')) 'Fixture-only loading must not reference HybridAStarPlanner.'
|
||||
$generatorSource = Get-Content -Raw -Encoding UTF8 (Join-Path $PSScriptRoot '..\ParkrobTrajplanner\PathSmoothing\Test\SmoothingFixtureGenerator.cs')
|
||||
Assert-True $generatorSource.Contains('CoarsePathPlanningService') 'Fixture generation must snapshot actual successful coarse-path planning outputs.'
|
||||
Assert-True $generatorSource.Contains('CoarsePathScenarioFactory') 'Fixture generation must use the established coarse-path scenarios where available.'
|
||||
|
||||
function Write-CorruptedFixture([scriptblock]$Mutate) {
|
||||
$temporaryPath = [IO.Path]::GetTempFileName()
|
||||
$document = Get-Content -Raw -Encoding UTF8 $FixturePath | ConvertFrom-Json
|
||||
& $Mutate $document
|
||||
[IO.File]::WriteAllText($temporaryPath, ($document | ConvertTo-Json -Depth 16), [Text.UTF8Encoding]::new($false))
|
||||
return $temporaryPath
|
||||
}
|
||||
|
||||
$coverageFixture = Write-CorruptedFixture { param($document) $document.scenarios[0].segments[0].endIndex = 1 }
|
||||
$directionFixture = Write-CorruptedFixture { param($document) $document.scenarios[0].segments[0].direction = 1 }
|
||||
$switchFixture = Write-CorruptedFixture { param($document) $document.scenarios[0].segments[0].endsAtGearSwitch = $true }
|
||||
try {
|
||||
Assert-ThrowsMatching { $loadMethod.Invoke($null, @($coverageFixture)) } 'Fixture path contract' 'Loader must reject segment gaps before accepting the fingerprint.'
|
||||
Assert-ThrowsMatching { $loadMethod.Invoke($null, @($directionFixture)) } 'Fixture path contract' 'Loader must reject segment direction mismatches before accepting the fingerprint.'
|
||||
Assert-ThrowsMatching { $loadMethod.Invoke($null, @($switchFixture)) } 'Fixture path contract' 'Loader must reject illegal gear-switch topology before accepting the fingerprint.'
|
||||
}
|
||||
finally {
|
||||
foreach ($temporaryPath in @($coverageFixture, $directionFixture, $switchFixture)) {
|
||||
if ($temporaryPath -and [IO.File]::Exists($temporaryPath)) { [IO.File]::Delete($temporaryPath) }
|
||||
}
|
||||
}
|
||||
|
||||
$generationPath = [IO.Path]::GetTempFileName()
|
||||
try {
|
||||
Assert-ThrowsMatching { $generateMethod.Invoke($null, @($generationPath, $false)) } '.+' 'Fixture generation must refuse to overwrite an existing target.'
|
||||
$generateMethod.Invoke($null, @($generationPath, $true))
|
||||
$generatedFixtures = $loadMethod.Invoke($null, @($generationPath))
|
||||
Assert-Equal 8 $generatedFixtures.Count 'Generated fixture data must remain loadable and retain all scenarios.'
|
||||
}
|
||||
finally {
|
||||
if ([IO.File]::Exists($generationPath)) { [IO.File]::Delete($generationPath) }
|
||||
}
|
||||
|
||||
Write-Output 'Path smoothing fixture checks passed.'
|
||||
@@ -0,0 +1,642 @@
|
||||
param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
|
||||
|
||||
$ErrorActionPreference = 'Stop'
|
||||
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
|
||||
|
||||
function Assert-True($Actual, [string]$Message) {
|
||||
if (-not $Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-Equal($Expected, $Actual, [string]$Message) {
|
||||
if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" }
|
||||
}
|
||||
|
||||
function Assert-Near([double]$Expected, [double]$Actual, [double]$Tolerance, [string]$Message) {
|
||||
if ([Math]::Abs($Expected - $Actual) -gt $Tolerance) {
|
||||
throw "$Message Expected=$Expected Actual=$Actual Tolerance=$Tolerance"
|
||||
}
|
||||
}
|
||||
|
||||
function Assert-False($Actual, [string]$Message) {
|
||||
if ($Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-Throws([scriptblock]$Action, [string]$Message) {
|
||||
try {
|
||||
& $Action
|
||||
}
|
||||
catch {
|
||||
return
|
||||
}
|
||||
throw $Message
|
||||
}
|
||||
|
||||
function Get-RequiredType([string]$Name) {
|
||||
return $assembly.GetType($Name, $true)
|
||||
}
|
||||
|
||||
function New-GeometryPoint(
|
||||
[double]$X,
|
||||
[double]$Y,
|
||||
[double]$ArcLength,
|
||||
[double]$Heading,
|
||||
[double]$UnwrappedHeading,
|
||||
[bool]$IsGearSwitch = $false) {
|
||||
return [Activator]::CreateInstance($pointType, @(
|
||||
$X, $Y, $ArcLength, $Heading, $UnwrappedHeading,
|
||||
[double]1.0, $IsGearSwitch, $anchor))
|
||||
}
|
||||
|
||||
function New-DirectionSegment(
|
||||
[int]$Index,
|
||||
$Direction,
|
||||
[object[]]$Points,
|
||||
[bool]$StartsAtGearSwitch = $false,
|
||||
[bool]$EndsAtGearSwitch = $false) {
|
||||
$typedPoints = [Array]::CreateInstance($pointType, $Points.Count)
|
||||
for ($pointIndex = 0; $pointIndex -lt $Points.Count; $pointIndex++) {
|
||||
$typedPoints.SetValue($Points[$pointIndex], $pointIndex)
|
||||
}
|
||||
|
||||
return [Activator]::CreateInstance($segmentType, @(
|
||||
$Index, $Direction, $typedPoints, $StartsAtGearSwitch, $EndsAtGearSwitch))
|
||||
}
|
||||
|
||||
function New-DirectionSegmentWithStartCurvature(
|
||||
[int]$Index,
|
||||
$Direction,
|
||||
[double]$StartVehicleCurvature,
|
||||
[object[]]$Points,
|
||||
[bool]$StartsAtGearSwitch = $false,
|
||||
[bool]$EndsAtGearSwitch = $false) {
|
||||
$typedPoints = [Array]::CreateInstance($pointType, $Points.Count)
|
||||
for ($pointIndex = 0; $pointIndex -lt $Points.Count; $pointIndex++) {
|
||||
$typedPoints.SetValue($Points[$pointIndex], $pointIndex)
|
||||
}
|
||||
|
||||
return [Activator]::CreateInstance($segmentType, @(
|
||||
$Index, $Direction, $typedPoints, $StartsAtGearSwitch, $EndsAtGearSwitch, $StartVehicleCurvature))
|
||||
}
|
||||
|
||||
function New-CoarsePoint(
|
||||
[double]$X,
|
||||
[double]$Y,
|
||||
[double]$ArcLength,
|
||||
$Direction,
|
||||
[bool]$IsGearSwitch = $false) {
|
||||
return [Activator]::CreateInstance($coarsePointType, @(
|
||||
$X, $Y, [double]0.0, [double]0.0, $ArcLength, $Direction,
|
||||
[double]0.0, [double]1.0, $IsGearSwitch, $coarseAnchor))
|
||||
}
|
||||
|
||||
function Invoke-Analysis([object[]]$Segments, [double]$Spacing = 0.05) {
|
||||
$typedSegments = [Array]::CreateInstance($segmentType, $Segments.Count)
|
||||
for ($index = 0; $index -lt $Segments.Count; $index++) {
|
||||
$typedSegments.SetValue($Segments[$index], $index)
|
||||
}
|
||||
|
||||
$arguments = [object[]]@($typedSegments, $Spacing, $null, $null)
|
||||
$accepted = $analyzeMethod.Invoke($analyzer, $arguments)
|
||||
$description = [string]::Join(',', @($Segments | ForEach-Object {
|
||||
"index=$($_.SegmentIndex);direction=$($_.Direction);points=$($_.Points.Count)"
|
||||
}))
|
||||
Assert-True $accepted ("Geometry analysis must accept the analytic candidate. Reason=" + $arguments[3] + '; Segments=' + $description)
|
||||
Assert-True ($null -ne $arguments[2]) 'Successful geometry analysis must return PathGeometryAnalysis.'
|
||||
return $arguments[2]
|
||||
}
|
||||
|
||||
function Assert-AnalysisRejected([object[]]$Segments, [string]$Message) {
|
||||
$typedSegments = [Array]::CreateInstance($segmentType, $Segments.Count)
|
||||
for ($segmentIndex = 0; $segmentIndex -lt $Segments.Count; $segmentIndex++) {
|
||||
$typedSegments.SetValue($Segments[$segmentIndex], $segmentIndex)
|
||||
}
|
||||
|
||||
$arguments = [object[]]@($typedSegments, [double]0.05, $null, $null)
|
||||
$accepted = $analyzeMethod.Invoke($analyzer, $arguments)
|
||||
Assert-True (-not $accepted) ($Message + '; Reason=' + $arguments[3])
|
||||
}
|
||||
|
||||
function Invoke-RejectedAnalysis([object[]]$Segments, [string]$Message) {
|
||||
$typedSegments = [Array]::CreateInstance($segmentType, $Segments.Count)
|
||||
for ($index = 0; $index -lt $Segments.Count; $index++) {
|
||||
$typedSegments.SetValue($Segments[$index], $index)
|
||||
}
|
||||
|
||||
$arguments = [object[]]@($typedSegments, [double]0.05, $null, $null)
|
||||
$accepted = $analyzeMethod.Invoke($analyzer, $arguments)
|
||||
Assert-False $accepted ($Message + '; Reason=' + $arguments[3])
|
||||
}
|
||||
|
||||
function New-CoarsePathPoint(
|
||||
[double]$X,
|
||||
[double]$Y,
|
||||
[double]$ArcLength,
|
||||
$Direction,
|
||||
[bool]$IsGearSwitch = $false,
|
||||
[string]$SourceName = 'MotionPrimitive',
|
||||
[double]$Heading = 0.0,
|
||||
[double]$VehicleCurvature = 0.0) {
|
||||
return [Activator]::CreateInstance($coarsePointType, @(
|
||||
$X, $Y, $Heading, $Heading, $ArcLength,
|
||||
$Direction, $VehicleCurvature, [double]1.0, $IsGearSwitch,
|
||||
[Enum]::Parse($coarsePointSourceType, $SourceName)))
|
||||
}
|
||||
|
||||
function New-EmptyGeometryMap {
|
||||
$mapRequest = [Activator]::CreateInstance($mapRequestType)
|
||||
$mapRequest.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000))
|
||||
$mapRequest.ResolutionMm = [single]50
|
||||
$mapRequest.AllowExplicitEmptyMap = $true
|
||||
$map = [Activator]::CreateInstance($mapFactoryType).Create($mapRequest).Map
|
||||
Assert-True ($null -ne $map) 'Geometry test must create an explicit empty planning map.'
|
||||
return $map
|
||||
}
|
||||
|
||||
function Invoke-GeometryValidation($Analysis, [object[]]$Segments, $Vehicle) {
|
||||
$typedSegments = [Array]::CreateInstance($segmentType, $Segments.Count)
|
||||
for ($index = 0; $index -lt $Segments.Count; $index++) {
|
||||
$typedSegments.SetValue($Segments[$index], $index)
|
||||
}
|
||||
|
||||
$preparedPath = [Activator]::CreateInstance($preparedPathType, [object[]](, $typedSegments))
|
||||
$arguments = [object[]]@(
|
||||
$Analysis.Path, $Analysis.Segments, $preparedPath, (New-EmptyGeometryMap), $Vehicle, [double]0.05,
|
||||
$null, [double]0.0, $null)
|
||||
$accepted = $validateMethod.Invoke($validator, $arguments)
|
||||
return [pscustomobject]@{ Accepted = $accepted; Reason = $arguments[8] }
|
||||
}
|
||||
|
||||
function New-CircularDirectionSegment(
|
||||
$Direction,
|
||||
[double]$Curvature,
|
||||
[int]$Intervals,
|
||||
[double]$ChordLength) {
|
||||
$radius = 1.0 / $Curvature
|
||||
$headingStep = 2.0 * [Math]::Asin($Curvature * $ChordLength / 2.0)
|
||||
$points = New-Object System.Collections.Generic.List[object]
|
||||
for ($index = 0; $index -le $Intervals; $index++) {
|
||||
$theta = $headingStep * $index
|
||||
$heading = if ($Direction.ToString() -eq 'Forward') { $theta } else { $theta + [Math]::PI }
|
||||
[void]$points.Add((New-GeometryPoint `
|
||||
(1.0 + $radius * [Math]::Sin($theta)) `
|
||||
(1.0 + $radius * (1.0 - [Math]::Cos($theta))) `
|
||||
($index * $ChordLength) $heading $heading))
|
||||
}
|
||||
return New-DirectionSegment 0 $Direction $points.ToArray()
|
||||
}
|
||||
|
||||
function Get-OldPolylineCurvatureMaximum($Analysis) {
|
||||
$maximum = 0.0
|
||||
$path = $Analysis.Path
|
||||
for ($index = 0; $index -lt $path.Count; $index++) {
|
||||
if ($path.Count -eq 1) {
|
||||
$curvature = 0.0
|
||||
}
|
||||
elseif ($index -eq 0) {
|
||||
$curvature = ($path[1].UnwrappedHeading - $path[0].UnwrappedHeading) /
|
||||
($path[1].ArcLength - $path[0].ArcLength)
|
||||
}
|
||||
elseif ($index -eq $path.Count - 1) {
|
||||
$curvature = ($path[$index].UnwrappedHeading - $path[$index - 1].UnwrappedHeading) /
|
||||
($path[$index].ArcLength - $path[$index - 1].ArcLength)
|
||||
}
|
||||
else {
|
||||
$curvature = ($path[$index + 1].UnwrappedHeading - $path[$index - 1].UnwrappedHeading) /
|
||||
($path[$index + 1].ArcLength - $path[$index - 1].ArcLength)
|
||||
}
|
||||
$maximum = [Math]::Max($maximum, [Math]::Abs($curvature))
|
||||
}
|
||||
return $maximum
|
||||
}
|
||||
|
||||
function Get-QuinticAnalyticCurvatureMaximum(
|
||||
[double]$C2,
|
||||
[double]$C3,
|
||||
[double]$C4,
|
||||
[double]$C5,
|
||||
[int]$ReferenceSamples = 20000) {
|
||||
$maximum = 0.0
|
||||
for ($index = 0; $index -lt $ReferenceSamples; $index++) {
|
||||
$t = $index / [double]($ReferenceSamples - 1)
|
||||
$firstDerivative = 2.0 * $C2 * $t + 3.0 * $C3 * $t * $t +
|
||||
4.0 * $C4 * $t * $t * $t + 5.0 * $C5 * $t * $t * $t * $t
|
||||
$secondDerivative = 2.0 * $C2 + 6.0 * $C3 * $t +
|
||||
12.0 * $C4 * $t * $t + 20.0 * $C5 * $t * $t * $t
|
||||
$curvature = [Math]::Abs($secondDerivative / [Math]::Pow(1.0 + $firstDerivative * $firstDerivative, 1.5))
|
||||
$maximum = [Math]::Max($maximum, $curvature)
|
||||
}
|
||||
return $maximum
|
||||
}
|
||||
|
||||
function New-QuinticDirectionSegment(
|
||||
[double]$C2,
|
||||
[double]$C3,
|
||||
[double]$C4,
|
||||
[double]$C5,
|
||||
[double]$DistributionPower,
|
||||
[int]$Samples = 400) {
|
||||
$points = New-Object System.Collections.Generic.List[object]
|
||||
$arcLength = 0.0
|
||||
$previousX = 0.0
|
||||
$previousY = 0.0
|
||||
for ($index = 0; $index -lt $Samples; $index++) {
|
||||
$t = [Math]::Pow($index / [double]($Samples - 1), $DistributionPower)
|
||||
$x = 1.0 + $t
|
||||
$y = $C2 * $t * $t + $C3 * $t * $t * $t + $C4 * $t * $t * $t * $t + $C5 * $t * $t * $t * $t * $t
|
||||
if ($index -gt 0) {
|
||||
$arcLength += [Math]::Sqrt(($x - $previousX) * ($x - $previousX) + ($y - $previousY) * ($y - $previousY))
|
||||
}
|
||||
$heading = [Math]::Atan2(
|
||||
2.0 * $C2 * $t + 3.0 * $C3 * $t * $t + 4.0 * $C4 * $t * $t * $t + 5.0 * $C5 * $t * $t * $t * $t,
|
||||
1.0)
|
||||
[void]$points.Add((New-GeometryPoint $x $y $arcLength $heading $heading))
|
||||
$previousX = $x
|
||||
$previousY = $y
|
||||
}
|
||||
return New-DirectionSegment 0 $forward $points.ToArray()
|
||||
}
|
||||
|
||||
$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
|
||||
$processing = $root + 'Processing.'
|
||||
$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
|
||||
|
||||
$analyzerType = Get-RequiredType ($processing + 'PathGeometryAnalyzer')
|
||||
$directionType = Get-RequiredType ($coarsePath + 'TravelDirection')
|
||||
$sourceType = Get-RequiredType ($root + 'SmoothedPathPointSource')
|
||||
$pointType = Get-RequiredType ($processing + 'SmoothingPoint2D')
|
||||
$segmentType = Get-RequiredType ($processing + 'PreparedDirectionSegment')
|
||||
$preparedPathType = Get-RequiredType ($processing + 'PreparedPath')
|
||||
$analysisType = Get-RequiredType ($processing + 'PathGeometryAnalysis')
|
||||
$preprocessorType = Get-RequiredType ($processing + 'PathSmoothingPreprocessor')
|
||||
$resamplerType = Get-RequiredType ($processing + 'ArcLengthResampler')
|
||||
$requestType = Get-RequiredType ($root + 'PathSmoothingRequest')
|
||||
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
|
||||
$coarsePointType = Get-RequiredType ($coarsePath + 'CoarsePathPoint')
|
||||
$coarseSegmentType = Get-RequiredType ($coarsePath + 'PathSegment')
|
||||
$coarseSourceType = Get-RequiredType ($coarsePath + 'CoarsePathPointSource')
|
||||
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
|
||||
$mapType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningGridMap'
|
||||
$coarsePointType = Get-RequiredType ($coarsePath + 'CoarsePathPoint')
|
||||
$coarseSegmentType = Get-RequiredType ($coarsePath + 'PathSegment')
|
||||
$coarsePointSourceType = Get-RequiredType ($coarsePath + 'CoarsePathPointSource')
|
||||
$smoothingRequestType = Get-RequiredType ($root + 'PathSmoothingRequest')
|
||||
$smoothingConfigurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
|
||||
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
|
||||
$boundsType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.MapBoundsMm'
|
||||
$mapRequestType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapRequest'
|
||||
$mapFactoryType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapFactory'
|
||||
$validatorType = Get-RequiredType ($root + 'Validation.SmoothedPathValidator')
|
||||
$rawBaselineBuilderType = Get-RequiredType ($processing + 'RawPathBaselineBuilder')
|
||||
|
||||
Assert-True ($null -ne $preparedPathType) 'PreparedPath must be discoverable for smoothing algorithms.'
|
||||
Assert-True ($null -ne $preprocessorType) 'PathSmoothingPreprocessor must be discoverable for request preparation.'
|
||||
Assert-True ($null -ne $resamplerType) 'ArcLengthResampler must be discoverable for deterministic resampling.'
|
||||
Assert-True ($null -ne $analysisType.GetProperty('MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter')) `
|
||||
'PathGeometryAnalysis must expose peak d-kappa/d-s.'
|
||||
|
||||
$analyzer = [Activator]::CreateInstance($analyzerType)
|
||||
$analyzeMethod = $analyzerType.GetMethod('TryAnalyze')
|
||||
Assert-True ($null -ne $analyzeMethod) 'PathGeometryAnalyzer must expose TryAnalyze.'
|
||||
Assert-Equal 4 $analyzeMethod.GetParameters().Length 'TryAnalyze must accept segments, spacing, analysis, and reason.'
|
||||
$validator = [Activator]::CreateInstance($validatorType)
|
||||
$validateMethod = $validatorType.GetMethod('TryValidate')
|
||||
Assert-True ($null -ne $validateMethod) 'SmoothedPathValidator must expose TryValidate.'
|
||||
Assert-Equal 9 $validateMethod.GetParameters().Length 'SmoothedPathValidator.TryValidate must retain its public contract.'
|
||||
$rawBaselineMethod = $rawBaselineBuilderType.GetMethod(
|
||||
'TryCreate',
|
||||
[Reflection.BindingFlags]'Static,NonPublic')
|
||||
Assert-True ($null -ne $rawBaselineMethod) 'RawPathBaselineBuilder must expose its internal TryCreate path.'
|
||||
|
||||
$forward = [Enum]::Parse($directionType, 'Forward')
|
||||
$reverse = [Enum]::Parse($directionType, 'Reverse')
|
||||
$anchor = [Enum]::Parse($sourceType, 'Anchor')
|
||||
$coarseAnchor = [Enum]::Parse($coarseSourceType, 'Start')
|
||||
|
||||
# The chord-corrected estimator must retain an exact circular curvature limit for both travel directions.
|
||||
$maximumAllowedCurvature = 5.0 / 6.0
|
||||
$circleVehicle = [Activator]::CreateInstance($vehicleType)
|
||||
$circleVehicle.LengthMeters = 0.20
|
||||
$circleVehicle.WidthMeters = 0.20
|
||||
$circleVehicle.SafetyMarginMeters = 0.0
|
||||
$circleVehicle.MaximumCurvaturePerMeter = $maximumAllowedCurvature
|
||||
foreach ($direction in @($forward, $reverse)) {
|
||||
$exactLimitCircle = New-CircularDirectionSegment $direction $maximumAllowedCurvature 20 0.05
|
||||
$exactLimitAnalysis = Invoke-Analysis @($exactLimitCircle)
|
||||
foreach ($point in $exactLimitAnalysis.Path) {
|
||||
Assert-True ([Math]::Abs($point.VehicleCurvature) -le $maximumAllowedCurvature + 1.0e-9) `
|
||||
('An exact-limit ' + $direction + ' circle must not exceed the curvature limit.')
|
||||
}
|
||||
$validation = Invoke-GeometryValidation $exactLimitAnalysis @($exactLimitCircle) $circleVehicle
|
||||
Assert-True $validation.Accepted ('The validator must accept an analyzed exact-limit ' + $direction + ' circle. Reason=' + $validation.Reason)
|
||||
}
|
||||
|
||||
# An analyzed over-limit circle must remain detectable by the unchanged validator threshold.
|
||||
$overLimitCircle = New-CircularDirectionSegment $forward ($maximumAllowedCurvature + 0.01) 20 0.05
|
||||
$overLimitAnalysis = Invoke-Analysis @($overLimitCircle)
|
||||
Assert-True ($overLimitAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter -gt $maximumAllowedCurvature + 1.0e-6) `
|
||||
'An over-limit circle must exceed the vehicle curvature limit by more than the validator tolerance.'
|
||||
$overLimitValidation = Invoke-GeometryValidation $overLimitAnalysis @($overLimitCircle) $circleVehicle
|
||||
Assert-False $overLimitValidation.Accepted 'The validator must reject an analyzed over-limit circle.'
|
||||
|
||||
# Trusted raw fallback must apply the exact vehicle-curvature gate before reconstruction.
|
||||
$trustedLimit = 0.80
|
||||
$trustedOverLimit = $trustedLimit + 0.0000005
|
||||
$analyzedCurvature = $trustedLimit + 0.01
|
||||
$chordLength = 0.05
|
||||
$headingStep = 2.0 * [Math]::Asin($analyzedCurvature * $chordLength / 2.0)
|
||||
$radius = 1.0 / $analyzedCurvature
|
||||
$trustedPoints = [Array]::CreateInstance($coarsePointType, 21)
|
||||
for ($index = 0; $index -le 20; $index++) {
|
||||
$theta = $headingStep * $index
|
||||
$trustedPoints.SetValue((New-CoarsePathPoint `
|
||||
(1.0 + $radius * [Math]::Sin($theta)) `
|
||||
(1.0 + $radius * (1.0 - [Math]::Cos($theta))) `
|
||||
($index * $chordLength) `
|
||||
$forward `
|
||||
$false `
|
||||
$(if ($index -eq 0) { 'Start' } else { 'MotionPrimitive' }) `
|
||||
$theta `
|
||||
$trustedOverLimit), $index)
|
||||
}
|
||||
$trustedSegments = [Array]::CreateInstance($coarseSegmentType, 1)
|
||||
$trustedSegments.SetValue(
|
||||
[Activator]::CreateInstance($coarseSegmentType, @(0, $forward, 0, 20, $false, $false)),
|
||||
0)
|
||||
$trustedVehicle = [Activator]::CreateInstance($vehicleType)
|
||||
$trustedVehicle.LengthMeters = 0.20
|
||||
$trustedVehicle.WidthMeters = 0.20
|
||||
$trustedVehicle.SafetyMarginMeters = 0.0
|
||||
$trustedVehicle.MaximumCurvaturePerMeter = $trustedLimit
|
||||
$trustedConfiguration = [Activator]::CreateInstance($smoothingConfigurationType)
|
||||
$trustedRequest = [Activator]::CreateInstance($smoothingRequestType, @(
|
||||
$trustedPoints,
|
||||
$trustedSegments,
|
||||
(New-EmptyGeometryMap),
|
||||
$trustedVehicle,
|
||||
$trustedConfiguration))
|
||||
$trustedPreprocessor = [Activator]::CreateInstance($preprocessorType)
|
||||
$trustedPrepareMethod = $preprocessorType.GetMethod('TryPrepare')
|
||||
$trustedPrepareArguments = [object[]]@($trustedRequest, $null, $null)
|
||||
Assert-True $trustedPrepareMethod.Invoke($trustedPreprocessor, $trustedPrepareArguments) `
|
||||
('Trusted raw fallback regression must prepare successfully. Reason=' + $trustedPrepareArguments[2])
|
||||
$trustedBaselineArguments = [object[]]@(
|
||||
$trustedRequest,
|
||||
$trustedPrepareArguments[1],
|
||||
$analyzer,
|
||||
$trustedConfiguration.OutputSpacingMeters,
|
||||
$validator,
|
||||
$trustedConfiguration.MaximumCollisionCheckStepMeters,
|
||||
$null,
|
||||
$null)
|
||||
Assert-False $rawBaselineMethod.Invoke($null, $trustedBaselineArguments) `
|
||||
'Trusted raw fallback must not publish a coarse curvature above the exact vehicle maximum.'
|
||||
Assert-Equal $overLimitValidation.Reason $trustedBaselineArguments[7] `
|
||||
'Rejecting trusted raw fallback must preserve the original analyzed-path validation failure.'
|
||||
|
||||
# The chord-corrected estimator must not under-estimate these smooth references more than the former polyline estimator.
|
||||
foreach ($quinticCase in @(
|
||||
[pscustomobject]@{ Name = 'SBend'; C2 = 0.0; C3 = 0.30; C4 = -0.45; C5 = 0.18; Power = 1.0 },
|
||||
[pscustomobject]@{ Name = 'EndpointPeak'; C2 = 0.18; C3 = -0.12; C4 = 0.0; C5 = 0.0; Power = 1.0 },
|
||||
[pscustomobject]@{ Name = 'NonUniformFinalInterval'; C2 = -0.12; C3 = 0.36; C4 = -0.30; C5 = 0.08; Power = 1.7 })) {
|
||||
$quintic = New-QuinticDirectionSegment $quinticCase.C2 $quinticCase.C3 $quinticCase.C4 $quinticCase.C5 $quinticCase.Power
|
||||
$quinticAnalysis = Invoke-Analysis @($quintic)
|
||||
$analyticMaximum = Get-QuinticAnalyticCurvatureMaximum $quinticCase.C2 $quinticCase.C3 $quinticCase.C4 $quinticCase.C5
|
||||
$newDeficit = [Math]::Max(0.0, $analyticMaximum - $quinticAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter)
|
||||
$oldDeficit = [Math]::Max(0.0, $analyticMaximum - (Get-OldPolylineCurvatureMaximum $quinticAnalysis))
|
||||
Assert-True ($newDeficit -le $oldDeficit + 1.0e-6) `
|
||||
($quinticCase.Name + ' must not have greater one-sided curvature under-estimation than the old polyline estimator.')
|
||||
}
|
||||
|
||||
# A half-turn over a single chord has no unambiguous geometric curvature estimate.
|
||||
$ambiguousTurn = New-DirectionSegment 0 $forward @(
|
||||
(New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
|
||||
(New-GeometryPoint 1.0 0.0 1.0 ([Math]::PI) ([Math]::PI)))
|
||||
Assert-AnalysisRejected @($ambiguousTurn) 'A two-point heading turn of π must be rejected as ambiguous.'
|
||||
|
||||
# Forward straight: resampling is exactly 0.05 m, preserves the exact endpoint, and has zero curvature.
|
||||
$straight = New-DirectionSegment 0 $forward @(
|
||||
(New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
|
||||
(New-GeometryPoint 1.0 0.0 1.0 0.0 0.0))
|
||||
$straightAnalysis = Invoke-Analysis @($straight)
|
||||
Assert-Equal 21 $straightAnalysis.Path.Count 'A one-metre straight must produce twenty 0.05 m intervals plus the initial point.'
|
||||
Assert-Near 0.0 $straightAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter 0.000000001 `
|
||||
'A straight must have zero peak d-kappa/d-s.'
|
||||
foreach ($point in $straightAnalysis.Path) {
|
||||
Assert-Near 0.0 $point.VehicleCurvatureDerivative 0.000000001 `
|
||||
'A straight point must carry zero d-kappa/d-s.'
|
||||
}
|
||||
for ($index = 1; $index -lt $straightAnalysis.Path.Count; $index++) {
|
||||
$left = $straightAnalysis.Path[$index - 1]
|
||||
$right = $straightAnalysis.Path[$index]
|
||||
$distance = [Math]::Sqrt(($right.X - $left.X) * ($right.X - $left.X) + ($right.Y - $left.Y) * ($right.Y - $left.Y))
|
||||
Assert-Near 0.05 $distance 0.000000001 'Straight resampling intervals must be exactly 0.05 m.'
|
||||
Assert-Near 0.0 $right.GeometricCurvature 0.000000001 'A forward straight must have zero geometric curvature.'
|
||||
Assert-Near 0.0 $right.VehicleCurvature 0.000000001 'A forward straight must have zero vehicle curvature.'
|
||||
}
|
||||
$straightEnd = $straightAnalysis.Path[$straightAnalysis.Path.Count - 1]
|
||||
Assert-Near 1.0 $straightEnd.X 0.0 'Resampling must retain the exact final X coordinate.'
|
||||
Assert-Near 0.0 $straightEnd.Y 0.0 'Resampling must retain the exact final Y coordinate.'
|
||||
|
||||
# Raw coarse anchors carry the vehicle pose, which may differ slightly from the chord tangent of a finite integration step.
|
||||
$poseAnchoredCurve = New-DirectionSegment 0 $forward @(
|
||||
(New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
|
||||
(New-GeometryPoint 1.0 0.2 1.0 0.4 0.4))
|
||||
$poseAnchoredAnalysis = Invoke-Analysis @($poseAnchoredCurve)
|
||||
Assert-Near 0.0 $poseAnchoredAnalysis.Path[0].Heading 0.000000000001 'Geometry analysis must retain the first coarse-anchor heading rather than replace it with a chord tangent.'
|
||||
$poseAnchoredEnd = $poseAnchoredAnalysis.Path[$poseAnchoredAnalysis.Path.Count - 1]
|
||||
Assert-Near 0.4 $poseAnchoredEnd.Heading 0.000000000001 'Geometry analysis must retain the final coarse-anchor heading rather than replace it with a chord tangent.'
|
||||
|
||||
# A forward R=2 quarter circle has positive +0.5 1/m vehicle curvature.
|
||||
$forwardArcPoints = New-Object System.Collections.Generic.List[object]
|
||||
for ($index = 0; $index -le 32; $index++) {
|
||||
$theta = ([Math]::PI / 2.0) * $index / 32.0
|
||||
$x = 2.0 * [Math]::Sin($theta)
|
||||
$y = 2.0 * (1.0 - [Math]::Cos($theta))
|
||||
$arcLength = 2.0 * $theta
|
||||
[void]$forwardArcPoints.Add((New-GeometryPoint $x $y $arcLength $theta $theta))
|
||||
}
|
||||
$forwardArc = New-DirectionSegment 0 $forward $forwardArcPoints.ToArray()
|
||||
$forwardArcAnalysis = Invoke-Analysis @($forwardArc)
|
||||
$forwardArcMidpoint = $forwardArcAnalysis.Path[[int]($forwardArcAnalysis.Path.Count / 2)]
|
||||
Assert-Near 0.5 $forwardArcMidpoint.GeometricCurvature 0.01 'An R=2 quarter circle must have geometric curvature +0.5 1/m.'
|
||||
Assert-Near 0.5 $forwardArcMidpoint.VehicleCurvature 0.01 'A forward R=2 quarter circle must have vehicle curvature +0.5 1/m.'
|
||||
|
||||
# The same spatial R=2 circle in reverse retains geometric curvature but negates vehicle curvature.
|
||||
$reverseArc = New-DirectionSegment 0 $reverse $forwardArcPoints.ToArray()
|
||||
$reverseArcAnalysis = Invoke-Analysis @($reverseArc)
|
||||
$reverseArcMidpoint = $reverseArcAnalysis.Path[[int]($reverseArcAnalysis.Path.Count / 2)]
|
||||
Assert-Near 0.5 $reverseArcMidpoint.GeometricCurvature 0.01 'Reverse travel must not change geometric curvature.'
|
||||
Assert-Near -0.5 $reverseArcMidpoint.VehicleCurvature 0.01 'A reverse R=2 quarter circle must have vehicle curvature -0.5 1/m.'
|
||||
|
||||
# Gear-switch poses are intentionally duplicated: they keep equal arc length and never enter a derivative denominator.
|
||||
$forwardBeforeSwitch = New-DirectionSegment 0 $forward @(
|
||||
(New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
|
||||
(New-GeometryPoint 1.0 0.0 1.0 0.0 0.0 $true)) $false $true
|
||||
$reverseAfterSwitch = New-DirectionSegment 1 $reverse @(
|
||||
(New-GeometryPoint 1.0 0.0 1.0 0.0 0.0 $true),
|
||||
(New-GeometryPoint 0.0 0.0 2.0 0.0 0.0)) $true $false
|
||||
$switchAnalysis = Invoke-Analysis @($forwardBeforeSwitch, $reverseAfterSwitch)
|
||||
$firstSegment = $switchAnalysis.Segments[0]
|
||||
$secondSegment = $switchAnalysis.Segments[1]
|
||||
$switchLeft = $switchAnalysis.Path[$firstSegment.EndIndex]
|
||||
$switchRight = $switchAnalysis.Path[$secondSegment.StartIndex]
|
||||
Assert-Near $switchLeft.X $switchRight.X 0.0 'Gear-switch endpoints must retain duplicate X coordinates.'
|
||||
Assert-Near $switchLeft.Y $switchRight.Y 0.0 'Gear-switch endpoints must retain duplicate Y coordinates.'
|
||||
Assert-Near $switchLeft.ArcLength $switchRight.ArcLength 0.0 'Gear-switch endpoints must retain duplicate arc length.'
|
||||
Assert-Equal 'Forward' $switchLeft.Direction.ToString() 'The first gear-switch pose must retain its forward segment direction.'
|
||||
Assert-Equal 'Reverse' $switchRight.Direction.ToString() 'The second gear-switch pose must retain its reverse segment direction.'
|
||||
Assert-True (-not [double]::IsNaN($switchLeft.GeometricCurvature)) 'No derivative may cross the gear-switch duplicate point.'
|
||||
Assert-True (-not [double]::IsNaN($switchRight.GeometricCurvature)) 'No reverse derivative may cross the gear-switch duplicate point.'
|
||||
Assert-True (-not [double]::IsNaN($switchLeft.VehicleCurvatureDerivative)) `
|
||||
'The forward side of a gear switch must have a finite one-sided derivative.'
|
||||
Assert-True (-not [double]::IsNaN($switchRight.VehicleCurvatureDerivative)) `
|
||||
'The reverse side of a gear switch must have a finite one-sided derivative.'
|
||||
|
||||
# Curvature at a curved segment's end and the following straight reverse segment's start must remain independently differentiated.
|
||||
$forwardArcToSwitch = New-DirectionSegment 0 $forward $forwardArcPoints.ToArray() $false $true
|
||||
$reverseStraightAfterArc = New-DirectionSegment 1 $reverse @(
|
||||
(New-GeometryPoint 2.0 2.0 0.0 ([Math]::PI / 2.0) ([Math]::PI / 2.0) $true),
|
||||
(New-GeometryPoint 2.0 1.0 1.0 ([Math]::PI / 2.0) ([Math]::PI / 2.0))) $true $false
|
||||
$curveSwitchAnalysis = Invoke-Analysis @($forwardArcToSwitch, $reverseStraightAfterArc)
|
||||
$reverseStraightStart = $curveSwitchAnalysis.Path[$curveSwitchAnalysis.Segments[1].StartIndex]
|
||||
Assert-Near 0.0 $reverseStraightStart.GeometricCurvature 0.000000001 'A gear-switch must not use the preceding curve to differentiate a reverse straight segment.'
|
||||
Assert-Near 0.0 $reverseStraightStart.VehicleCurvatureDerivative 0.000000001 `
|
||||
'No curvature derivative may cross from the preceding forward arc into a reverse straight.'
|
||||
|
||||
$physicalStart = New-DirectionSegmentWithStartCurvature 0 $forward 0.20 @(
|
||||
(New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
|
||||
(New-GeometryPoint 1.0 0.0 1.0 0.0 0.0))
|
||||
$physicalStartAnalysis = Invoke-Analysis @($physicalStart)
|
||||
Assert-Near 0.20 $physicalStartAnalysis.Path[0].VehicleCurvature 0.000000001 `
|
||||
'The unified analyzer must retain a real start steering-curvature boundary state.'
|
||||
Assert-True ($physicalStartAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter -gt 0.0) `
|
||||
'A real start-curvature mismatch must remain visible to the quality analyzer.'
|
||||
|
||||
# A boundary that claims a gear switch must be a duplicated pose with opposite direction; discontinuities are rejected.
|
||||
$invalidSwitch = New-DirectionSegment 1 $reverse @(
|
||||
(New-GeometryPoint 1.2 0.0 1.0 0.0 0.0 $true),
|
||||
(New-GeometryPoint 0.2 0.0 2.0 0.0 0.0)) $true $false
|
||||
Assert-AnalysisRejected @($forwardBeforeSwitch, $invalidSwitch) 'A discontinuous gear-switch boundary must be rejected.'
|
||||
$trailingGearSwitch = New-DirectionSegment 0 $forward @(
|
||||
(New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
|
||||
(New-GeometryPoint 1.0 0.0 1.0 0.0 0.0)) $false $true
|
||||
Assert-AnalysisRejected @($trailingGearSwitch) 'The final direction segment must not advertise a non-existent trailing gear switch.'
|
||||
|
||||
# The public preprocessor receives a raw coarse path and resets every prepared direction segment to local arc length zero.
|
||||
$coarsePoints = [Array]::CreateInstance($coarsePointType, 4)
|
||||
$coarsePoints.SetValue((New-CoarsePoint 0.0 0.0 0.0 $forward), 0)
|
||||
$coarsePoints.SetValue((New-CoarsePoint 1.0 0.0 1.0 $forward), 1)
|
||||
$coarsePoints.SetValue((New-CoarsePoint 1.0 0.0 1.0 $reverse $true), 2)
|
||||
$coarsePoints.SetValue((New-CoarsePoint 0.0 0.0 2.0 $reverse), 3)
|
||||
$coarseSegments = [Array]::CreateInstance($coarseSegmentType, 2)
|
||||
$coarseSegments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(0, $forward, 0, 1, $false, $true)), 0)
|
||||
$coarseSegments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(1, $reverse, 2, 3, $true, $false)), 1)
|
||||
$vehicle = [Activator]::CreateInstance($vehicleType)
|
||||
$vehicle.LengthMeters = 1.0
|
||||
$vehicle.WidthMeters = 0.5
|
||||
$vehicle.SafetyMarginMeters = 0.0
|
||||
$vehicle.MaximumCurvaturePerMeter = 1.0
|
||||
$configuration = [Activator]::CreateInstance($configurationType)
|
||||
$uninitializedMap = [System.Runtime.Serialization.FormatterServices]::GetUninitializedObject($mapType)
|
||||
$request = [Activator]::CreateInstance($requestType, @($coarsePoints, $coarseSegments, $uninitializedMap, $vehicle, $configuration))
|
||||
$preprocessor = [Activator]::CreateInstance($preprocessorType)
|
||||
$prepareMethod = $preprocessorType.GetMethod('TryPrepare')
|
||||
$prepareArguments = [object[]]@($request, $null, $null)
|
||||
$prepared = $prepareMethod.Invoke($preprocessor, $prepareArguments)
|
||||
Assert-True $prepared ('Preprocessor must accept a legal forward/reverse raw coarse path. Reason=' + $prepareArguments[2])
|
||||
Assert-Near 0.0 $prepareArguments[1].Segments[1].Points[0].ArcLength 0.0 'Every prepared direction segment must begin at local arc length zero.'
|
||||
Assert-True $prepareArguments[1].Segments[1].Points[0].IsGearSwitchPoint 'The reverse prepared segment must retain its gear-switch point.'
|
||||
|
||||
# Finite coordinates can still overflow distance arithmetic; public resampling must reject them instead of emitting NaN/Infinity.
|
||||
$overflowSegment = New-DirectionSegment 0 $forward @(
|
||||
(New-GeometryPoint -1.0e308 0.0 0.0 0.0 0.0),
|
||||
(New-GeometryPoint 1.0e308 0.0 1.0 0.0 0.0))
|
||||
$resampler = [Activator]::CreateInstance($resamplerType)
|
||||
$segmentResampleMethod = @($resamplerType.GetMethods() | Where-Object {
|
||||
$_.Name -eq 'TryResample' -and $_.GetParameters()[0].ParameterType -eq $segmentType
|
||||
})[0]
|
||||
$resampleArguments = [object[]]@($overflowSegment, [double]0.05, $null, $null)
|
||||
$resampled = $segmentResampleMethod.Invoke($resampler, $resampleArguments)
|
||||
Assert-True (-not $resampled) ('Resampling must reject a distance overflow. Reason=' + $resampleArguments[3])
|
||||
|
||||
# A prepared path must reject null direction segments rather than silently dropping them during flattening.
|
||||
$nullSegmentArray = [Array]::CreateInstance($segmentType, 1)
|
||||
$nullSegmentRejected = $false
|
||||
try { [void][Activator]::CreateInstance($preparedPathType, @($nullSegmentArray)) } catch { $nullSegmentRejected = $true }
|
||||
Assert-True $nullSegmentRejected 'PreparedPath must reject a null direction segment.'
|
||||
|
||||
# Unwrapped heading must not jump by 2π when tangents cross the -π/π branch cut.
|
||||
$crossing = New-DirectionSegment 0 $forward @(
|
||||
(New-GeometryPoint 0.0 0.0 0.0 (170.0 * [Math]::PI / 180.0) (170.0 * [Math]::PI / 180.0)),
|
||||
(New-GeometryPoint -1.0 ([Math]::Tan(10.0 * [Math]::PI / 180.0)) 1.015 (170.0 * [Math]::PI / 180.0) (170.0 * [Math]::PI / 180.0)),
|
||||
(New-GeometryPoint -2.0 0.0 2.03 (-170.0 * [Math]::PI / 180.0) (-170.0 * [Math]::PI / 180.0)))
|
||||
$crossingAnalysis = Invoke-Analysis @($crossing)
|
||||
for ($index = 1; $index -lt $crossingAnalysis.Path.Count; $index++) {
|
||||
$difference = [Math]::Abs($crossingAnalysis.Path[$index].UnwrappedHeading - $crossingAnalysis.Path[$index - 1].UnwrappedHeading)
|
||||
Assert-True ($difference -lt [Math]::PI) 'Unwrapped headings must remain continuous across the ±π branch cut.'
|
||||
}
|
||||
|
||||
# The request preprocessor must reset arc length independently for every direction segment,
|
||||
# while retaining the duplicated pose that represents a legal forward-to-reverse gear switch.
|
||||
$preprocessor = [Activator]::CreateInstance($preprocessorType)
|
||||
$prepareMethod = $preprocessorType.GetMethod('TryPrepare')
|
||||
Assert-True ($null -ne $prepareMethod) 'PathSmoothingPreprocessor must expose TryPrepare.'
|
||||
$coarsePath = [Array]::CreateInstance($coarsePointType, 5)
|
||||
$coarsePath.SetValue((New-CoarsePathPoint 0.0 0.0 0.0 $forward $false 'Start'), 0)
|
||||
$coarsePath.SetValue((New-CoarsePathPoint 1.0 0.0 1.0 $forward), 1)
|
||||
$coarsePath.SetValue((New-CoarsePathPoint 2.0 0.0 2.0 $forward), 2)
|
||||
$coarsePath.SetValue((New-CoarsePathPoint 2.0 0.0 2.0 $reverse $true), 3)
|
||||
$coarsePath.SetValue((New-CoarsePathPoint 1.0 0.0 3.0 $reverse), 4)
|
||||
$coarseSegments = [Array]::CreateInstance($coarseSegmentType, 2)
|
||||
$coarseSegments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(0, $forward, 0, 2, $false, $true)), 0)
|
||||
$coarseSegments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(1, $reverse, 3, 4, $true, $false)), 1)
|
||||
$vehicle = [Activator]::CreateInstance($vehicleType)
|
||||
$vehicle.LengthMeters = [double]0.80
|
||||
$vehicle.WidthMeters = [double]0.60
|
||||
$vehicle.SafetyMarginMeters = [double]0.05
|
||||
$vehicle.MaximumCurvaturePerMeter = [double]0.80
|
||||
$configuration = [Activator]::CreateInstance($smoothingConfigurationType)
|
||||
$smoothingRequest = [Activator]::CreateInstance($smoothingRequestType, @(
|
||||
$coarsePath, $coarseSegments, (New-EmptyGeometryMap), $vehicle, $configuration))
|
||||
$prepareArguments = [object[]]@($smoothingRequest, $null, $null)
|
||||
Assert-True $prepareMethod.Invoke($preprocessor, $prepareArguments) ('Preprocessor must accept legal forward/reverse topology. Reason=' + $prepareArguments[2])
|
||||
$preparedPath = $prepareArguments[1]
|
||||
Assert-Equal 2 $preparedPath.Segments.Count 'Preprocessor must preserve both direction segments.'
|
||||
Assert-Near 0.0 $preparedPath.Segments[1].Points[0].ArcLength 0.0 'The reverse segment must restart local arc length at zero.'
|
||||
Assert-True $preparedPath.Segments[1].Points[0].IsGearSwitchPoint 'The duplicate reverse gear-switch point must be retained.'
|
||||
|
||||
# Segments may meet only at a paired, coincident forward/reverse gear switch.
|
||||
$illegalGearJump = New-DirectionSegment 1 $reverse @(
|
||||
(New-GeometryPoint 1.25 0.0 1.0 0.0 0.0 $true),
|
||||
(New-GeometryPoint 0.25 0.0 2.0 0.0 0.0)) $true $false
|
||||
Invoke-RejectedAnalysis @($forwardBeforeSwitch, $illegalGearJump) 'A gear-switch boundary whose poses differ must be rejected.'
|
||||
$illegalNormalBoundary = New-DirectionSegment 1 $forward @(
|
||||
(New-GeometryPoint 2.0 0.0 0.0 0.0 0.0),
|
||||
(New-GeometryPoint 3.0 0.0 1.0 0.0 0.0)) $false $false
|
||||
Invoke-RejectedAnalysis @($straight, $illegalNormalBoundary) 'A non-gear segment boundary must be rejected.'
|
||||
|
||||
# Finite endpoint coordinates can still overflow while computing their separation; reject before interpolation.
|
||||
$resampler = [Activator]::CreateInstance($resamplerType)
|
||||
$resamplePointsMethod = $resamplerType.GetMethods() | Where-Object {
|
||||
$_.Name -eq 'TryResample' -and $_.GetParameters().Length -eq 4 -and
|
||||
$_.GetParameters()[0].ParameterType -eq [System.Collections.Generic.IReadOnlyList``1].MakeGenericType($pointType)
|
||||
} | Select-Object -First 1
|
||||
Assert-True ($null -ne $resamplePointsMethod) 'ArcLengthResampler must expose point-list TryResample.'
|
||||
$hugePoints = [Array]::CreateInstance($pointType, 2)
|
||||
$hugeCoordinate = [double]::MaxValue / 2.0
|
||||
$hugePoints.SetValue((New-GeometryPoint (-$hugeCoordinate) 0.0 0.0 0.0 0.0), 0)
|
||||
$hugePoints.SetValue((New-GeometryPoint $hugeCoordinate 0.0 1.0 0.0 0.0), 1)
|
||||
$resampleArguments = [object[]]@($hugePoints, [double]0.05, $null, $null)
|
||||
Assert-False $resamplePointsMethod.Invoke($resampler, $resampleArguments) 'Resampling must reject an infinite geometric distance caused by finite coordinates.'
|
||||
|
||||
# PreparedPath is an all-or-nothing immutable topology snapshot: null direction segments are invalid.
|
||||
$nullPreparedSegments = [Array]::CreateInstance($segmentType, 1)
|
||||
Assert-Throws { [Activator]::CreateInstance($preparedPathType, @($nullPreparedSegments)) } 'PreparedPath must reject null direction segments.'
|
||||
|
||||
# Segment indices are deliberately dense and equal to their position in the candidate array.
|
||||
$sparseSegment = New-DirectionSegment 2 $forward @(
|
||||
(New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
|
||||
(New-GeometryPoint 1.0 0.0 1.0 0.0 0.0))
|
||||
Invoke-RejectedAnalysis @($sparseSegment) 'Prepared direction-segment indices must match their dense array position.'
|
||||
|
||||
Write-Output 'Path smoothing geometry checks passed.'
|
||||
@@ -0,0 +1,140 @@
|
||||
param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
|
||||
|
||||
$ErrorActionPreference = 'Stop'
|
||||
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
|
||||
|
||||
function Assert-True($Actual, [string]$Message) { if (-not $Actual) { throw $Message } }
|
||||
function Assert-Equal($Expected, $Actual, [string]$Message) {
|
||||
if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" }
|
||||
}
|
||||
function Assert-Near([double]$Expected, [double]$Actual, [string]$Message) {
|
||||
if ([Math]::Abs($Expected - $Actual) -gt 0.000001) { throw "$Message Expected=$Expected Actual=$Actual" }
|
||||
}
|
||||
function Get-RequiredType([string]$Name) { return $assembly.GetType($Name, $true) }
|
||||
|
||||
function Assert-CoarsePathGeometry($ExpectedPath, $ActualPath, $Map, [string]$ScenarioName) {
|
||||
Assert-Equal $ExpectedPath.Count $ActualPath.Count "Raw comparison request $ScenarioName must preserve every coarse-path point."
|
||||
for ($index = 0; $index -lt $ExpectedPath.Count; $index++) {
|
||||
$expected = $ExpectedPath[$index]
|
||||
$actual = $ActualPath[$index]
|
||||
Assert-Near $expected.X $actual.X "Raw comparison request $ScenarioName point $index must preserve X."
|
||||
Assert-Near $expected.Y $actual.Y "Raw comparison request $ScenarioName point $index must preserve Y."
|
||||
Assert-Near $expected.Heading $actual.Heading "Raw comparison request $ScenarioName point $index must preserve heading."
|
||||
Assert-Near $expected.UnwrappedHeading $actual.UnwrappedHeading "Raw comparison request $ScenarioName point $index must preserve unwrapped heading."
|
||||
Assert-Near $expected.ArcLength $actual.ArcLength "Raw comparison request $ScenarioName point $index must preserve arc length."
|
||||
Assert-Equal $expected.Direction $actual.Direction "Raw comparison request $ScenarioName point $index must preserve travel direction."
|
||||
Assert-Near $expected.VehicleCurvature $actual.VehicleCurvature "Raw comparison request $ScenarioName point $index must preserve vehicle curvature."
|
||||
$expectedClearance = $expected.BodyClearance
|
||||
if ([double]::IsPositiveInfinity($expectedClearance)) {
|
||||
$widthMeters = ($Map.Bounds.XMax - $Map.Bounds.XMin) / 1000.0
|
||||
$heightMeters = ($Map.Bounds.YMax - $Map.Bounds.YMin) / 1000.0
|
||||
$expectedClearance = [Math]::Sqrt($widthMeters * $widthMeters + $heightMeters * $heightMeters)
|
||||
}
|
||||
Assert-Near $expectedClearance $actual.BodyClearance "Raw comparison request $ScenarioName point $index must preserve or normalize clearance for smoothing."
|
||||
Assert-Equal $expected.IsGearSwitchPoint $actual.IsGearSwitchPoint "Raw comparison request $ScenarioName point $index must preserve gear-switch flag."
|
||||
Assert-Equal $expected.Source $actual.Source "Raw comparison request $ScenarioName point $index must preserve point source."
|
||||
}
|
||||
}
|
||||
|
||||
function Assert-SegmentTopology($ExpectedSegments, $ActualPath, $ActualSegments, [string]$Description) {
|
||||
Assert-Equal $ExpectedSegments.Count $ActualSegments.Count "$Description must preserve segment count."
|
||||
$expectedStartIndex = 0
|
||||
for ($index = 0; $index -lt $ActualSegments.Count; $index++) {
|
||||
$expected = $ExpectedSegments[$index]
|
||||
$actual = $ActualSegments[$index]
|
||||
Assert-Equal $index $actual.SegmentIndex "$Description segment $index must retain its stable index."
|
||||
Assert-Equal $expected.Direction $actual.Direction "$Description segment $index must preserve travel direction."
|
||||
Assert-Equal $expected.StartsAtGearSwitch $actual.StartsAtGearSwitch "$Description segment $index must preserve start gear-switch topology."
|
||||
Assert-Equal $expected.EndsAtGearSwitch $actual.EndsAtGearSwitch "$Description segment $index must preserve end gear-switch topology."
|
||||
Assert-Equal $expectedStartIndex $actual.StartIndex "$Description segment $index must start directly after the prior segment."
|
||||
Assert-True ($actual.EndIndex -ge $actual.StartIndex -and $actual.EndIndex -lt $ActualPath.Count) "$Description segment $index must cover valid path indices."
|
||||
Assert-Equal $actual.StartsAtGearSwitch $ActualPath[$actual.StartIndex].IsGearSwitchPoint "$Description segment $index start flag must match its path point."
|
||||
for ($pointIndex = $actual.StartIndex; $pointIndex -le $actual.EndIndex; $pointIndex++) {
|
||||
Assert-Equal $actual.Direction $ActualPath[$pointIndex].Direction "$Description segment $index may not contain mixed directions."
|
||||
}
|
||||
$hasNext = $index + 1 -lt $ActualSegments.Count
|
||||
$expectedEndSwitch = $hasNext -and $ActualPath[$actual.EndIndex + 1].IsGearSwitchPoint
|
||||
Assert-Equal $expectedEndSwitch $actual.EndsAtGearSwitch "$Description segment $index end flag must match the next gear switch."
|
||||
$expectedStartIndex = $actual.EndIndex + 1
|
||||
}
|
||||
Assert-Equal $ActualPath.Count $expectedStartIndex "$Description segments must cover every path point."
|
||||
}
|
||||
|
||||
function Assert-GearSwitchGeometry($ExpectedPath, $ActualPath, [string]$Description) {
|
||||
$expectedSwitches = @($ExpectedPath | Where-Object { $_.IsGearSwitchPoint })
|
||||
$actualSwitches = @($ActualPath | Where-Object { $_.IsGearSwitchPoint })
|
||||
Assert-Equal $expectedSwitches.Count $actualSwitches.Count "$Description must preserve gear-switch count."
|
||||
for ($index = 0; $index -lt $expectedSwitches.Count; $index++) {
|
||||
Assert-Near $expectedSwitches[$index].X $actualSwitches[$index].X "$Description gear switch $index must preserve X."
|
||||
Assert-Near $expectedSwitches[$index].Y $actualSwitches[$index].Y "$Description gear switch $index must preserve Y."
|
||||
Assert-Near $expectedSwitches[$index].Heading $actualSwitches[$index].Heading "$Description gear switch $index must preserve heading."
|
||||
Assert-Equal $expectedSwitches[$index].Direction $actualSwitches[$index].Direction "$Description gear switch $index must preserve direction."
|
||||
}
|
||||
}
|
||||
|
||||
function Assert-RawBaselinePath($ExpectedPath, $ActualPath, [string]$ScenarioName) {
|
||||
Assert-Equal $ExpectedPath.Count $ActualPath.Count "Raw baseline $ScenarioName must preserve every coarse-path point."
|
||||
for ($index = 0; $index -lt $ExpectedPath.Count; $index++) {
|
||||
$expected = $ExpectedPath[$index]
|
||||
$actual = $ActualPath[$index]
|
||||
Assert-Near $expected.X $actual.X "Raw baseline $ScenarioName point $index must preserve X."
|
||||
Assert-Near $expected.Y $actual.Y "Raw baseline $ScenarioName point $index must preserve Y."
|
||||
Assert-Near $expected.Heading $actual.Heading "Raw baseline $ScenarioName point $index must preserve heading."
|
||||
Assert-Near $expected.UnwrappedHeading $actual.UnwrappedHeading "Raw baseline $ScenarioName point $index must preserve unwrapped heading."
|
||||
Assert-Near $expected.ArcLength $actual.ArcLength "Raw baseline $ScenarioName point $index must preserve arc length."
|
||||
Assert-Equal $expected.Direction $actual.Direction "Raw baseline $ScenarioName point $index must preserve travel direction."
|
||||
Assert-Near $expected.VehicleCurvature $actual.VehicleCurvature "Raw baseline $ScenarioName point $index must preserve vehicle curvature."
|
||||
Assert-Equal $expected.IsGearSwitchPoint $actual.IsGearSwitchPoint "Raw baseline $ScenarioName point $index must preserve gear-switch topology."
|
||||
}
|
||||
}
|
||||
|
||||
$coarse = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
|
||||
$coarseFacade = $coarse + 'Facade.'
|
||||
$coarseTest = $coarse + 'Test.'
|
||||
$smoothingTest = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.Test.'
|
||||
$smoothingFacade = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade.'
|
||||
$comparison = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison.'
|
||||
|
||||
$coarseServiceType = Get-RequiredType ($coarseFacade + 'CoarsePathPlanningService')
|
||||
$coarseJobType = Get-RequiredType ($coarseFacade + 'CoarsePathPlanningJob')
|
||||
$coarseResultType = Get-RequiredType ($coarseFacade + 'CoarsePathPlanningJobResult')
|
||||
$scenarioType = Get-RequiredType ($coarseTest + 'CoarsePathTestScenario')
|
||||
$coarseFactoryType = Get-RequiredType ($coarseTest + 'CoarsePathScenarioFactory')
|
||||
$scenarioFactoryType = Get-RequiredType ($smoothingTest + 'SmoothingScenarioFactory')
|
||||
$comparisonServiceType = Get-RequiredType ($smoothingFacade + 'PathSmoothingComparisonService')
|
||||
$comparisonRequestType = Get-RequiredType ($comparison + 'PathSmoothingComparisonRequest')
|
||||
|
||||
$coarseCreate = $coarseFactoryType.GetMethod('Create', [Type[]]@($scenarioType))
|
||||
$coarsePlan = $coarseServiceType.GetMethod('Plan', [Type[]]@($coarseJobType, [Threading.CancellationToken]))
|
||||
$createComparisonRequest = $scenarioFactoryType.GetMethod('CreateEndToEndRequest', [Type[]]@($coarseJobType, $coarseResultType))
|
||||
$compare = $comparisonServiceType.GetMethod('Compare', [Type[]]@($comparisonRequestType, [Threading.CancellationToken]))
|
||||
Assert-True ($null -ne $createComparisonRequest) 'Smoothing scenario factory must convert a successful coarse planning job into a comparison request.'
|
||||
|
||||
$planner = [Activator]::CreateInstance($coarseServiceType)
|
||||
$comparisonService = [Activator]::CreateInstance($comparisonServiceType)
|
||||
foreach ($scenarioName in @('ExplicitEmpty', 'RectangleDetour', 'ManualAndTwoLeg', 'ReverseGearSwitch')) {
|
||||
$scenario = [Enum]::Parse($scenarioType, $scenarioName)
|
||||
$job = $coarseCreate.Invoke($null, @($scenario))
|
||||
$planningTimer = [Diagnostics.Stopwatch]::StartNew()
|
||||
$planned = $coarsePlan.Invoke($planner, @($job, [Threading.CancellationToken]::None))
|
||||
$planningTimer.Stop()
|
||||
Assert-Equal 'Success' $planned.PlanningResult.Status.ToString() "Coarse scenario $scenarioName must succeed before smoothing comparison."
|
||||
$request = $createComparisonRequest.Invoke($null, @($job, $planned))
|
||||
Assert-CoarsePathGeometry $planned.PlanningResult.Path $request.SmoothingRequest.CoarsePath $planned.MapResult.Map $scenarioName
|
||||
$comparisonTimer = [Diagnostics.Stopwatch]::StartNew()
|
||||
$comparisonResult = $compare.Invoke($comparisonService, @($request, [Threading.CancellationToken]::None))
|
||||
$comparisonTimer.Stop()
|
||||
Assert-True (-not $comparisonResult.IsCancelled) "Comparison $scenarioName must not be cancelled."
|
||||
Assert-Equal 'Success' $comparisonResult.RawPathBaseline.Status.ToString() "Raw baseline $scenarioName must remain a feasible, verified copy of the coarse path."
|
||||
Assert-RawBaselinePath $planned.PlanningResult.Path $comparisonResult.RawPathBaseline.Path $scenarioName
|
||||
Assert-SegmentTopology $planned.PlanningResult.Segments $comparisonResult.RawPathBaseline.Path $comparisonResult.RawPathBaseline.Segments "Raw baseline $scenarioName"
|
||||
Assert-GearSwitchGeometry $planned.PlanningResult.Path $comparisonResult.RawPathBaseline.Path "Raw baseline $scenarioName"
|
||||
foreach ($entry in $comparisonResult.Entries | Where-Object { $_.Status.ToString() -eq 'Success' }) {
|
||||
Assert-SegmentTopology $comparisonResult.RawPathBaseline.Segments $entry.Path $entry.Segments "Successful $($entry.Method) $scenarioName"
|
||||
Assert-GearSwitchGeometry $comparisonResult.RawPathBaseline.Path $entry.Path "Successful $($entry.Method) $scenarioName"
|
||||
}
|
||||
$successfulEntryCount = @($comparisonResult.Entries | Where-Object { $_.Status.ToString() -eq 'Success' }).Count
|
||||
Write-Output ("$scenarioName diagnostics: planning=$([Math]::Round($planningTimer.Elapsed.TotalSeconds, 3))s; comparison=$([Math]::Round($comparisonTimer.Elapsed.TotalSeconds, 3))s; successfulMethods=$successfulEntryCount")
|
||||
}
|
||||
|
||||
Write-Output 'Path smoothing end-to-end integration checks passed.'
|
||||
@@ -0,0 +1,252 @@
|
||||
param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
|
||||
|
||||
$ErrorActionPreference = 'Stop'
|
||||
$newtonsoft = Join-Path $env:USERPROFILE '.nuget\packages\newtonsoft.json\13.0.4\lib\netstandard2.0\Newtonsoft.Json.dll'
|
||||
if (Test-Path $newtonsoft) { $null = [Reflection.Assembly]::LoadFrom($newtonsoft) }
|
||||
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
|
||||
|
||||
function Assert-True($Actual, [string]$Message) {
|
||||
if (-not $Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-Equal($Expected, $Actual, [string]$Message) {
|
||||
if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" }
|
||||
}
|
||||
|
||||
function Assert-Near([double]$Expected, [double]$Actual, [double]$Tolerance, [string]$Message) {
|
||||
if ([Math]::Abs($Expected - $Actual) -gt $Tolerance) {
|
||||
throw "$Message Expected=$Expected Actual=$Actual Tolerance=$Tolerance"
|
||||
}
|
||||
}
|
||||
|
||||
function Get-RequiredType([string]$Name) {
|
||||
return $assembly.GetType($Name, $true)
|
||||
}
|
||||
|
||||
function Get-InternalProperty($Instance, [string]$Name) {
|
||||
return $Instance.GetType().GetProperty(
|
||||
$Name,
|
||||
[Reflection.BindingFlags]'Public,NonPublic,Instance').GetValue($Instance)
|
||||
}
|
||||
|
||||
function Get-InternalMethod($Type, [string]$Name) {
|
||||
return @($Type.GetMethods([Reflection.BindingFlags]'Public,NonPublic,Instance,Static') |
|
||||
Where-Object Name -eq $Name)[0]
|
||||
}
|
||||
|
||||
function New-InternalInstance($Type) {
|
||||
return [Activator]::CreateInstance(
|
||||
$Type,
|
||||
[Reflection.BindingFlags]'Instance,NonPublic,Public',
|
||||
$null,
|
||||
@(),
|
||||
$null)
|
||||
}
|
||||
|
||||
$builderType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2.LocalG2CandidateBuilder'
|
||||
$hooksType = $builderType.GetNestedType('TestHooks', [Reflection.BindingFlags]'Public,NonPublic')
|
||||
Assert-True ($null -ne $hooksType) 'LocalG2CandidateBuilder must expose narrowly scoped deterministic TestHooks.'
|
||||
$executeMethod = $hooksType.GetMethod('Execute', [Reflection.BindingFlags]'Public,Static')
|
||||
Assert-True ($null -ne $executeMethod) 'TestHooks must execute deterministic candidate scenarios.'
|
||||
|
||||
function Invoke-Scenario([string]$Scenario) {
|
||||
return $executeMethod.Invoke($null, @($Scenario))
|
||||
}
|
||||
|
||||
$isolated = Invoke-Scenario 'Isolated'
|
||||
Assert-True ($isolated.CandidateCount -gt 0 -and $isolated.CandidateCount -le 12) `
|
||||
'An isolated event must produce a bounded non-empty candidate set.'
|
||||
Assert-Near 0.0 $isolated.StartPositionError 1e-9 'Window start position must match.'
|
||||
Assert-Near 0.0 $isolated.EndPositionError 1e-9 'Window end position must match.'
|
||||
Assert-Near 0.0 $isolated.StartCurvatureError 1e-8 'Window start curvature must match.'
|
||||
Assert-Near 0.0 $isolated.EndCurvatureError 1e-8 'Window end curvature must match.'
|
||||
Assert-True $isolated.ContainsLocalG2Source 'Generated samples must identify their source.'
|
||||
|
||||
$cluster = Invoke-Scenario 'Cluster'
|
||||
Assert-Equal 1 $cluster.OutputRegionCount 'Overlapping events must be built as one region.'
|
||||
Assert-True $cluster.InternalConnectionsAreG2 'Internal anchors must share tangent and curvature.'
|
||||
|
||||
$reverse = Invoke-Scenario 'Reverse'
|
||||
Assert-Equal 'Reverse' $reverse.Direction 'Reverse candidates must preserve segment direction.'
|
||||
Assert-True $reverse.VehicleAndGeometricCurvatureSignsAreOpposite `
|
||||
'Reverse candidates must convert vehicle curvature to geometric curvature exactly once.'
|
||||
|
||||
$spliced = Invoke-Scenario 'Spliced'
|
||||
Assert-True $spliced.NoDuplicateNonGearPoints 'Splicing must remove duplicate boundary samples.'
|
||||
Assert-True $spliced.EndpointsUnchanged 'Splicing must preserve the complete segment endpoints.'
|
||||
|
||||
$startBoundary = Invoke-Scenario 'StartBoundary'
|
||||
Assert-True ($startBoundary.CandidateCount -gt 0) 'A start-boundary event must build a one-sided candidate.'
|
||||
Assert-Near 0.0 $startBoundary.StartCurvatureError 1e-8 `
|
||||
'A start-boundary candidate must use the preserved physical start vehicle curvature.'
|
||||
|
||||
$interiorDerivative = Invoke-Scenario 'InteriorStationaryCurve'
|
||||
Assert-True $interiorDerivative.Rejected `
|
||||
'A curve with a stationary interior derivative must be rejected even when its endpoint chord is short.'
|
||||
|
||||
$constantVelocity = Invoke-Scenario 'ConstantVelocityCurve'
|
||||
Assert-True $constantVelocity.Accepted `
|
||||
'A nonstationary constant-velocity curve with collinear derivative controls must be accepted.'
|
||||
|
||||
$exactSplice = Invoke-Scenario 'ExactSpliceEndpoints'
|
||||
Assert-True $exactSplice.EndpointsAreExact `
|
||||
'Splicing must write interpolated exact endpoints instead of accepting approximate candidate endpoints.'
|
||||
|
||||
$gearBoundary = Invoke-Scenario 'GearBoundary'
|
||||
Assert-True $gearBoundary.GearBoundaryMarkerPreserved `
|
||||
'A window touching a gear-switch segment boundary must preserve its point-level gear marker.'
|
||||
|
||||
$twoRegions = Invoke-Scenario 'TwoRegionWorkOrder'
|
||||
Assert-True $twoRegions.WorkOrderDescending `
|
||||
'Same-segment regions must be processed from larger original local arc to smaller local arc.'
|
||||
Assert-True $twoRegions.FrontArcPreservedAfterBackReplacement `
|
||||
'Replacing the back region must preserve the front region original arc coordinates.'
|
||||
Assert-True $twoRegions.BothReplacementsRetained `
|
||||
'Back-then-front replacement must retain the exact LocalG2Transition interiors at (0.75, 0.20) and (2.25, 0.20).'
|
||||
Assert-True $twoRegions.ForwardOrderRejected `
|
||||
'The regression fixture must prove that front-first invalidates the original back absolute arc.'
|
||||
Assert-True $twoRegions.DeterministicWorkOrder `
|
||||
'Work ordering must repeat exactly without mutating report order.'
|
||||
Assert-True $twoRegions.DeterministicReplacementGeometry `
|
||||
'Repeated back-then-front replacement must preserve count and every point X, Y, source, and direction.'
|
||||
Assert-True $twoRegions.InvalidWorkOrderRejected `
|
||||
'Work ordering must reject null regions and cross-segment event contents.'
|
||||
|
||||
$ordering = Invoke-Scenario 'MultiSegmentWorkOrder'
|
||||
Assert-True $ordering.SegmentOrderAscending `
|
||||
'Region work ordering must process segment indices in ascending order.'
|
||||
Assert-True $ordering.EqualArcUsesReportOrder `
|
||||
'Equal first-local-arc regions must retain their original report-index order.'
|
||||
|
||||
$evaluatorType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2.LocalG2CandidateEvaluator'
|
||||
$evaluatorHooksType = $evaluatorType.GetNestedType('TestHooks', [Reflection.BindingFlags]'Public,NonPublic')
|
||||
Assert-True ($null -ne $evaluatorHooksType) 'LocalG2CandidateEvaluator must expose narrowly scoped deterministic TestHooks.'
|
||||
$evaluateMethod = $evaluatorHooksType.GetMethod('Execute', [Reflection.BindingFlags]'Public,Static')
|
||||
Assert-True ($null -ne $evaluateMethod) 'TestHooks must execute deterministic candidate quality-gate scenarios.'
|
||||
|
||||
function Invoke-EvaluationScenario([string]$Scenario) {
|
||||
return $evaluateMethod.Invoke($null, @($Scenario))
|
||||
}
|
||||
|
||||
$tooFar = Invoke-EvaluationScenario 'TooFar'
|
||||
Assert-Equal 'DeviationExceeded' $tooFar.FailureReason `
|
||||
'A collision-free candidate more than 0.10 m from the raw window must be rejected.'
|
||||
|
||||
$overshoot = Invoke-EvaluationScenario 'Overshoot'
|
||||
Assert-Equal 'CurvatureOvershoot' $overshoot.FailureReason `
|
||||
'A candidate outside the raw regional curvature range must be rejected.'
|
||||
|
||||
$noOp = Invoke-EvaluationScenario 'NoOp'
|
||||
Assert-Equal 'InsufficientImprovement' $noOp.FailureReason `
|
||||
'A safe no-op must not be accepted.'
|
||||
|
||||
$oscillating = Invoke-EvaluationScenario 'Oscillating'
|
||||
Assert-Equal 'VariationCostRegression' $oscillating.FailureReason `
|
||||
'Repeated curvature oscillation must fail the variation-cost gate.'
|
||||
|
||||
$lowClearance = Invoke-EvaluationScenario 'LowClearance'
|
||||
Assert-Equal 'InsufficientClearance' $lowClearance.FailureReason `
|
||||
'A path with less than 0.02 m checked clearance must be rejected.'
|
||||
|
||||
$improved = Invoke-EvaluationScenario 'Improved'
|
||||
Assert-Equal 'Accepted' $improved.Status `
|
||||
'A safe candidate with at least 20 percent peak improvement must be accepted.'
|
||||
|
||||
$smallestDeviation = Invoke-EvaluationScenario 'SmallestDeviation'
|
||||
$best = Invoke-EvaluationScenario 'Best'
|
||||
Assert-Equal $smallestDeviation.CandidateIndex $best.CandidateIndex `
|
||||
'Among sufficient candidates, minimum deviation must win before extra smoothness.'
|
||||
|
||||
# Exercise the real builder → evaluator seam that a hand-built TestHook candidate does not cover.
|
||||
$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
|
||||
$factoryType = Get-RequiredType ($root + 'Test.SmoothingScenarioFactory')
|
||||
$requestType = Get-RequiredType ($root + 'PathSmoothingRequest')
|
||||
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
|
||||
$methodType = Get-RequiredType ($root + 'SmoothingMethod')
|
||||
$preprocessorType = Get-RequiredType ($root + 'Processing.PathSmoothingPreprocessor')
|
||||
$optionsType = Get-RequiredType ($root + 'LocalG2.LocalG2OptionsSnapshot')
|
||||
$detectorType = Get-RequiredType ($root + 'LocalG2.CurvatureTransitionDetector')
|
||||
$plannerType = Get-RequiredType ($root + 'LocalG2.LocalG2WindowPlanner')
|
||||
$fixturePath = Join-Path $PSScriptRoot '..\ParkrobTrajplanner\PathSmoothing\Test\Fixtures\path-smoothing-fixtures.json'
|
||||
$fixtures = (Get-InternalMethod $factoryType 'CreateFixtureRequests').Invoke($null, @((Resolve-Path $fixturePath).Path))
|
||||
$baseRequest = $fixtures[1].SmoothingRequest # single-turn
|
||||
$configuration = [Activator]::CreateInstance($configurationType)
|
||||
$configuration.Method = [Enum]::Parse($methodType, 'LocalG2Quintic')
|
||||
$singleTurnRequest = [Activator]::CreateInstance($requestType, @(
|
||||
$baseRequest.CoarsePath, $baseRequest.Segments, $baseRequest.Map, $baseRequest.Vehicle, $configuration))
|
||||
$preprocessor = [Activator]::CreateInstance($preprocessorType)
|
||||
$prepareArgs = [object[]]@($singleTurnRequest, $null, $null)
|
||||
Assert-True ((Get-InternalMethod $preprocessorType 'TryPrepare').Invoke($preprocessor, $prepareArgs)) `
|
||||
'SingleTurn must prepare before evaluating Local G2 candidates.'
|
||||
$preparedPath = $prepareArgs[1]
|
||||
$options = [Activator]::CreateInstance($optionsType, [Reflection.BindingFlags]'Instance,NonPublic,Public', $null, @($configuration), $null)
|
||||
$detector = New-InternalInstance $detectorType
|
||||
$detectArgs = [object[]]@($singleTurnRequest, [double]$singleTurnRequest.Vehicle.MaximumCurvaturePerMeter, $options, $null, $null)
|
||||
Assert-True ((Get-InternalMethod $detectorType 'TryDetect').Invoke($detector, $detectArgs)) `
|
||||
'SingleTurn must detect its Local G2 transition.'
|
||||
$planner = New-InternalInstance $plannerType
|
||||
$planArgs = [object[]]@($preparedPath, $detectArgs[3], $options, $null, $null)
|
||||
Assert-True ((Get-InternalMethod $plannerType 'TryPlan').Invoke($planner, $planArgs)) `
|
||||
'SingleTurn must plan a Local G2 region.'
|
||||
$region = $null
|
||||
foreach ($plannedRegion in $planArgs[3]) { $region = $plannedRegion; break }
|
||||
$preparedSegment = (Get-InternalProperty $preparedPath 'Segments')[(Get-InternalProperty $region 'SegmentIndex')]
|
||||
$realBuilder = New-InternalInstance $builderType
|
||||
$realCandidates = (Get-InternalMethod $builderType 'Build').Invoke($realBuilder, @(
|
||||
$preparedSegment, $region, [double]$configuration.OutputSpacingMeters, $options, [Threading.CancellationToken]::None))
|
||||
Assert-True ($realCandidates.Count -gt 0) 'SingleTurn must build real Local G2 candidates.'
|
||||
$realEvaluator = New-InternalInstance $evaluatorType
|
||||
$duplicateFailures = @()
|
||||
$acceptedCandidates = @()
|
||||
$extractWindow = $evaluatorType.GetMethod('TryExtractWindow', [Reflection.BindingFlags]'NonPublic,Static')
|
||||
$analyzeWindow = $evaluatorType.GetMethod('TryAnalyzeWindow', [Reflection.BindingFlags]'NonPublic,Instance')
|
||||
foreach ($realCandidate in $realCandidates) {
|
||||
$evaluation = (Get-InternalMethod $evaluatorType 'Evaluate').Invoke($realEvaluator, @(
|
||||
$preparedPath, $preparedPath, $region, $realCandidate, $singleTurnRequest, $options, [Threading.CancellationToken]::None))
|
||||
$failureReason = Get-InternalProperty $evaluation 'FailureReason'
|
||||
if ($failureReason -eq 2) {
|
||||
$candidateIndex = Get-InternalProperty $realCandidate 'CandidateIndex'
|
||||
$duplicateFailures += $candidateIndex
|
||||
}
|
||||
if (Get-InternalProperty $evaluation 'Accepted') {
|
||||
$acceptedCandidates += (Get-InternalProperty $realCandidate 'CandidateIndex')
|
||||
}
|
||||
$windowArgs = [object[]]@($preparedSegment,
|
||||
(Get-InternalProperty $realCandidate 'StartArcLengthMeters'),
|
||||
(Get-InternalProperty $realCandidate 'EndArcLengthMeters'), $null, $null)
|
||||
$null = $extractWindow.Invoke($null, $windowArgs)
|
||||
$rawWindowArgs = [object[]]::new(5)
|
||||
$rawWindowArgs[0] = $preparedSegment
|
||||
$rawWindowArgs[1] = $windowArgs[3]
|
||||
$rawWindowArgs[2] = [double]$configuration.OutputSpacingMeters
|
||||
$candidateWindowArgs = [object[]]::new(5)
|
||||
$candidateWindowArgs[0] = $preparedSegment
|
||||
$candidateWindowArgs[1] = $realCandidate.GetType().GetProperty(
|
||||
'RegionPoints', [Reflection.BindingFlags]'Public,NonPublic,Instance').GetValue($realCandidate)
|
||||
$candidateWindowArgs[2] = [double]$configuration.OutputSpacingMeters
|
||||
$null = $analyzeWindow.Invoke($realEvaluator, $rawWindowArgs)
|
||||
$null = $analyzeWindow.Invoke($realEvaluator, $candidateWindowArgs)
|
||||
$rawPath = Get-InternalProperty $rawWindowArgs[3] 'Path'
|
||||
$rawMinimum = [double]::PositiveInfinity
|
||||
$rawMaximum = [double]::NegativeInfinity
|
||||
foreach ($point in $rawPath) {
|
||||
$rawMinimum = [Math]::Min($rawMinimum, $point.VehicleCurvature)
|
||||
$rawMaximum = [Math]::Max($rawMaximum, $point.VehicleCurvature)
|
||||
}
|
||||
$rawPeak = Get-InternalProperty $rawWindowArgs[3] 'MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter'
|
||||
$candidatePeak = Get-InternalProperty $candidateWindowArgs[3] 'MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter'
|
||||
$candidateMaximum = Get-InternalProperty $candidateWindowArgs[3] 'MaximumAbsoluteVehicleCurvaturePerMeter'
|
||||
$candidateIndex = Get-InternalProperty $realCandidate 'CandidateIndex'
|
||||
$fraction = @(0.00, 0.10, 0.20)[[Math]::Floor($candidateIndex / 4)]
|
||||
$scale = @(1.00, 1.20, 1.40, 1.60)[$candidateIndex % 4]
|
||||
Write-Output ('SWEEP candidate={0}; alpha={1:F2}; internalScale={2:F2}; window={3:R}..{4:R}; result={5}; rawK=[{6:R},{7:R}]; maxK={8:R}; rawPeak={9:R}; peak={10:R}' -f
|
||||
$candidateIndex, $fraction, $scale,
|
||||
(Get-InternalProperty $realCandidate 'StartArcLengthMeters'), (Get-InternalProperty $realCandidate 'EndArcLengthMeters'),
|
||||
$failureReason, $rawMinimum, $rawMaximum, $candidateMaximum, $rawPeak, $candidatePeak)
|
||||
}
|
||||
Assert-Equal 0 $duplicateFailures.Count `
|
||||
'SingleTurn builder candidates must not fail evaluator window analysis due to duplicate or degenerate points.'
|
||||
Assert-True ($acceptedCandidates.Count -gt 0) `
|
||||
'SingleTurn must produce at least one builder candidate accepted by the unchanged evaluator gates.'
|
||||
|
||||
Write-Output 'Path smoothing Local G2 candidate checks passed.'
|
||||
@@ -0,0 +1,118 @@
|
||||
param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
|
||||
|
||||
$ErrorActionPreference = 'Stop'
|
||||
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
|
||||
|
||||
function Assert-True($Actual, [string]$Message) {
|
||||
if (-not $Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-Near([double]$Expected, [double]$Actual, [double]$Tolerance, [string]$Message) {
|
||||
if ([Math]::Abs($Expected - $Actual) -gt $Tolerance) {
|
||||
throw "$Message Expected=$Expected Actual=$Actual Tolerance=$Tolerance"
|
||||
}
|
||||
}
|
||||
|
||||
function Assert-Rejected($Result, [string]$Message) {
|
||||
if ($Result.Accepted) { throw $Message }
|
||||
Assert-True (-not [string]::IsNullOrWhiteSpace($Result.Reason)) 'Rejected curves must provide a stable reason.'
|
||||
}
|
||||
|
||||
function Assert-Throws([scriptblock]$Action, [string]$Message) {
|
||||
try {
|
||||
& $Action
|
||||
}
|
||||
catch [ArgumentOutOfRangeException] {
|
||||
return
|
||||
}
|
||||
catch [System.Reflection.TargetInvocationException] {
|
||||
Assert-True ($_.Exception.InnerException -is [ArgumentOutOfRangeException]) $Message
|
||||
return
|
||||
}
|
||||
throw $Message
|
||||
}
|
||||
|
||||
$curveType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2.QuinticHermiteCurve2D', $true)
|
||||
$tryCreate = $curveType.GetMethod('TryCreate', [Reflection.BindingFlags]'Static,NonPublic')
|
||||
$evaluate = $curveType.GetMethod('Evaluate', [Reflection.BindingFlags]'Instance,NonPublic')
|
||||
Assert-True ($null -ne $tryCreate) 'QuinticHermiteCurve2D must expose its internal TryCreate factory.'
|
||||
Assert-True ($null -ne $evaluate) 'QuinticHermiteCurve2D must expose its internal Evaluate method.'
|
||||
|
||||
function Invoke-Curve(
|
||||
[double]$X0, [double]$Y0, [double]$Dx0, [double]$Dy0, [double]$Ddx0, [double]$Ddy0,
|
||||
[double]$X1, [double]$Y1, [double]$Dx1, [double]$Dy1, [double]$Ddx1, [double]$Ddy1) {
|
||||
$arguments = [object[]]@($X0, $Y0, $Dx0, $Dy0, $Ddx0, $Ddy0, $X1, $Y1, $Dx1, $Dy1, $Ddx1, $Ddy1, $null, $null)
|
||||
$accepted = $tryCreate.Invoke($null, $arguments)
|
||||
Assert-True $accepted "Quintic Hermite curve creation must succeed. Reason=$($arguments[13])"
|
||||
return $arguments[12]
|
||||
}
|
||||
|
||||
function Invoke-InvalidCurve([string]$CaseName) {
|
||||
$arguments = switch ($CaseName) {
|
||||
'ZeroFirstDerivative' { [object[]]@(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2.0, 0.0, 1.0, 0.0, 0.0, 0.0, $null, $null) }
|
||||
'NonFiniteInput' { [object[]]@(0.0, 0.0, [double]::NaN, 0.0, 0.0, 0.0, 2.0, 0.0, 1.0, 0.0, 0.0, 0.0, $null, $null) }
|
||||
default { throw "Unknown invalid curve case: $CaseName" }
|
||||
}
|
||||
return [PSCustomObject]@{
|
||||
Accepted = $tryCreate.Invoke($null, $arguments)
|
||||
Reason = [string]$arguments[13]
|
||||
}
|
||||
}
|
||||
|
||||
function Invoke-Evaluate($Curve, [double]$U) {
|
||||
$arguments = [object[]]@($U, $null, $null, $null, $null, $null, $null)
|
||||
$evaluate.Invoke($Curve, $arguments)
|
||||
return [PSCustomObject]@{
|
||||
X = [double]$arguments[1]; Y = [double]$arguments[2]
|
||||
Dx = [double]$arguments[3]; Dy = [double]$arguments[4]
|
||||
Ddx = [double]$arguments[5]; Ddy = [double]$arguments[6]
|
||||
}
|
||||
}
|
||||
|
||||
function Get-Curvature($Sample) {
|
||||
return ($Sample.Dx * $Sample.Ddy - $Sample.Dy * $Sample.Ddx) /
|
||||
[Math]::Pow($Sample.Dx * $Sample.Dx + $Sample.Dy * $Sample.Dy, 1.5)
|
||||
}
|
||||
|
||||
function Assert-Boundary($Expected, $Actual, [string]$Name) {
|
||||
Assert-Near $Expected.X $Actual.X 1e-10 "$Name X must match."
|
||||
Assert-Near $Expected.Y $Actual.Y 1e-10 "$Name Y must match."
|
||||
Assert-Near $Expected.Dx $Actual.Dx 1e-10 "$Name dX must match."
|
||||
Assert-Near $Expected.Dy $Actual.Dy 1e-10 "$Name dY must match."
|
||||
Assert-Near $Expected.Ddx $Actual.Ddx 1e-10 "$Name ddX must match."
|
||||
Assert-Near $Expected.Ddy $Actual.Ddy 1e-10 "$Name ddY must match."
|
||||
}
|
||||
|
||||
$straight = Invoke-Curve 0 0 1 0 0 0 2 0 1 0 0 0
|
||||
$straightStart = Invoke-Evaluate $straight 0.0
|
||||
$straightEnd = Invoke-Evaluate $straight 1.0
|
||||
$straightMid = Invoke-Evaluate $straight 0.5
|
||||
Assert-Boundary ([PSCustomObject]@{ X = 0.0; Y = 0.0; Dx = 1.0; Dy = 0.0; Ddx = 0.0; Ddy = 0.0 }) $straightStart 'Straight start'
|
||||
Assert-Boundary ([PSCustomObject]@{ X = 2.0; Y = 0.0; Dx = 1.0; Dy = 0.0; Ddx = 0.0; Ddy = 0.0 }) $straightEnd 'Straight end'
|
||||
Assert-Near 0.0 $straightMid.Y 1e-12 'A straight Hermite curve must remain on the axis.'
|
||||
|
||||
$curved = Invoke-Curve 0 0 1 0 0 0.4 1 1 0 1 -0.4 0
|
||||
$curvedStart = Invoke-Evaluate $curved 0.0
|
||||
$curvedEnd = Invoke-Evaluate $curved 1.0
|
||||
Assert-Boundary ([PSCustomObject]@{ X = 0.0; Y = 0.0; Dx = 1.0; Dy = 0.0; Ddx = 0.0; Ddy = 0.4 }) $curvedStart 'Curved start'
|
||||
Assert-Boundary ([PSCustomObject]@{ X = 1.0; Y = 1.0; Dx = 0.0; Dy = 1.0; Ddx = -0.4; Ddy = 0.0 }) $curvedEnd 'Curved end'
|
||||
Assert-Near 0.4 (Get-Curvature $curvedStart) 1e-10 'Start curvature must match boundary derivatives.'
|
||||
Assert-Near 0.4 (Get-Curvature $curvedEnd) 1e-10 'End curvature must match boundary derivatives.'
|
||||
|
||||
$allFinite = $true
|
||||
foreach ($u in @(0.0, 0.25, 0.5, 0.75, 1.0)) {
|
||||
$sample = Invoke-Evaluate $curved $u
|
||||
foreach ($value in @($sample.X, $sample.Y, $sample.Dx, $sample.Dy, $sample.Ddx, $sample.Ddy)) {
|
||||
$allFinite = $allFinite -and (-not [double]::IsNaN($value)) -and (-not [double]::IsInfinity($value))
|
||||
}
|
||||
}
|
||||
Assert-True $allFinite 'All sampled values must be finite.'
|
||||
|
||||
Assert-Rejected (Invoke-InvalidCurve 'ZeroFirstDerivative') `
|
||||
'A curve endpoint with zero first derivative must be rejected.'
|
||||
Assert-Rejected (Invoke-InvalidCurve 'NonFiniteInput') `
|
||||
'A curve with a non-finite boundary value must be rejected.'
|
||||
Assert-Throws { Invoke-Evaluate $curved -0.001 } 'Evaluate must reject parameters below zero.'
|
||||
Assert-Throws { Invoke-Evaluate $curved 1.001 } 'Evaluate must reject parameters above one.'
|
||||
|
||||
Write-Output 'Local G2 quintic curve checks passed.'
|
||||
@@ -0,0 +1,129 @@
|
||||
param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
|
||||
|
||||
$ErrorActionPreference = 'Stop'
|
||||
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
|
||||
|
||||
function Assert-True($Actual, [string]$Message) {
|
||||
if (-not $Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-False($Actual, [string]$Message) {
|
||||
if ($Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-Equal($Expected, $Actual, [string]$Message) {
|
||||
if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" }
|
||||
}
|
||||
|
||||
function Assert-Near([double]$Expected, [double]$Actual, [double]$Tolerance, [string]$Message) {
|
||||
if ([Math]::Abs($Expected - $Actual) -gt $Tolerance) {
|
||||
throw "$Message Expected=$Expected Actual=$Actual Tolerance=$Tolerance"
|
||||
}
|
||||
}
|
||||
|
||||
function Get-RequiredType([string]$Name) {
|
||||
return $assembly.GetType($Name, $true)
|
||||
}
|
||||
|
||||
$detectorType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2.CurvatureTransitionDetector'
|
||||
$hooksType = $detectorType.GetNestedType('TestHooks', [Reflection.BindingFlags]'Public,NonPublic')
|
||||
Assert-True ($null -ne $hooksType) 'CurvatureTransitionDetector must expose its narrowly scoped nested TestHooks helper.'
|
||||
$executeMethod = $hooksType.GetMethod('Execute', [Reflection.BindingFlags]'Public,Static')
|
||||
Assert-True ($null -ne $executeMethod) 'TestHooks must expose deterministic scenario execution for reflection tests.'
|
||||
|
||||
function Invoke-Scenario([string]$Scenario) {
|
||||
return $executeMethod.Invoke($null, @($Scenario))
|
||||
}
|
||||
|
||||
# Same direction: 0 -> 0.4167 is detected once.
|
||||
$singleTransition = Invoke-Scenario 'SingleTransition'
|
||||
Assert-Equal 1 $singleTransition.TransitionCount 'One primitive curvature jump must be detected.'
|
||||
Assert-Near 0.4167 $singleTransition.MaximumJump 0.0001 'The jump magnitude must be retained.'
|
||||
|
||||
# Same pose at a forward/reverse boundary: no event crosses the stop.
|
||||
$gearSwitch = Invoke-Scenario 'GearSwitch'
|
||||
Assert-Equal 0 $gearSwitch.TransitionCount 'A stopped gear switch must not be a smoothing event.'
|
||||
|
||||
# A 0.01 1/m numerical change is below max(0.001, 5% of 0.8333).
|
||||
$noise = Invoke-Scenario 'Noise'
|
||||
Assert-Equal 0 $noise.TransitionCount 'Sub-threshold curvature noise must be ignored.'
|
||||
|
||||
# Two 0.50 m windows whose ranges overlap are merged.
|
||||
$overlap = Invoke-Scenario 'Overlap'
|
||||
Assert-Equal 1 $overlap.RegionCount 'Overlapping windows must form one joint region.'
|
||||
Assert-Equal 2 $overlap.TransitionCountInFirstRegion 'The merged region must retain both events.'
|
||||
|
||||
# Near a segment start, the window becomes asymmetric without crossing the hard boundary.
|
||||
$nearStart = Invoke-Scenario 'NearStart'
|
||||
Assert-Near 0.0 $nearStart.StartArcLength 0.000000001 'A start window must be clamped to the segment.'
|
||||
Assert-True ($nearStart.RightWindowLength -gt $nearStart.LeftWindowLength) `
|
||||
'Unavailable left length must be shifted to the right.'
|
||||
|
||||
# Finite endpoint curvatures may still overflow during subtraction; detection must fail rather than publish infinity.
|
||||
$coarsePathPointType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.CoarsePath.CoarsePathPoint'
|
||||
$pathSegmentType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.CoarsePath.PathSegment'
|
||||
$directionType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.CoarsePath.TravelDirection'
|
||||
$coarseSourceType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.CoarsePath.CoarsePathPointSource'
|
||||
$requestType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.PathSmoothing.PathSmoothingRequest'
|
||||
$configurationType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.PathSmoothing.PathSmoothingConfiguration'
|
||||
$optionsType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2.LocalG2OptionsSnapshot'
|
||||
$forward = [Enum]::Parse($directionType, 'Forward')
|
||||
$motionPrimitive = [Enum]::Parse($coarseSourceType, 'MotionPrimitive')
|
||||
$overflowPoints = [Array]::CreateInstance($coarsePathPointType, 2)
|
||||
$overflowPoints.SetValue([Activator]::CreateInstance($coarsePathPointType, @(
|
||||
[double]0.0, [double]0.0, [double]0.0, [double]0.0, [double]0.0, $forward,
|
||||
-[double]::MaxValue, [double]1.0, $false, $motionPrimitive)), 0)
|
||||
$overflowPoints.SetValue([Activator]::CreateInstance($coarsePathPointType, @(
|
||||
[double]0.1, [double]0.0, [double]0.0, [double]0.0, [double]0.1, $forward,
|
||||
[double]::MaxValue, [double]1.0, $false, $motionPrimitive)), 1)
|
||||
$overflowSegments = [Array]::CreateInstance($pathSegmentType, 1)
|
||||
$overflowSegments.SetValue([Activator]::CreateInstance($pathSegmentType, @(0, $forward, 0, 1, $false, $false)), 0)
|
||||
$configuration = [Activator]::CreateInstance($configurationType)
|
||||
$optionsConstructor = $optionsType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null, @($configurationType), $null)
|
||||
Assert-True ($null -ne $optionsConstructor) 'Local G2 options must be constructible for detector reflection tests.'
|
||||
$options = $optionsConstructor.Invoke(@($configuration))
|
||||
$overflowRequest = [Activator]::CreateInstance($requestType, @($overflowPoints, $overflowSegments, $null, $null, $configuration))
|
||||
$detector = [Activator]::CreateInstance($detectorType, $true)
|
||||
$tryDetect = $detectorType.GetMethod('TryDetect', [Reflection.BindingFlags]'Instance,NonPublic')
|
||||
Assert-True ($null -ne $tryDetect) 'CurvatureTransitionDetector must retain its internal TryDetect contract.'
|
||||
$overflowArguments = [object[]]@($overflowRequest, [double]0.8333, $options, $null, $null)
|
||||
$overflowAccepted = $tryDetect.Invoke($detector, $overflowArguments)
|
||||
Assert-False $overflowAccepted 'Curvature subtraction overflow must reject detection.'
|
||||
Assert-True (-not [string]::IsNullOrWhiteSpace([string]$overflowArguments[4])) `
|
||||
'Rejected overflow detection must provide a stable reason.'
|
||||
Assert-Equal 0 $overflowArguments[3].Count 'Rejected overflow detection must not publish a curvature event.'
|
||||
|
||||
$plannerType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2.LocalG2WindowPlanner'
|
||||
$plannerHooksType = $plannerType.GetNestedType('TestHooks', [Reflection.BindingFlags]'Public,NonPublic')
|
||||
Assert-True ($null -ne $plannerHooksType) 'LocalG2WindowPlanner must expose narrow deterministic TestHooks.'
|
||||
$planScenario = $plannerHooksType.GetMethod('Execute', [Reflection.BindingFlags]'Public,Static')
|
||||
|
||||
$separated = $planScenario.Invoke($null, @('SeparatedByOneMeter'))
|
||||
Assert-Equal 2 $separated.RegionCount 'Events 1.0 m apart cannot share a 0.80 m total window.'
|
||||
Assert-Equal '1,1' $separated.TransitionCounts 'Separated events must remain one event per region.'
|
||||
|
||||
$mergeable = $planScenario.Invoke($null, @('Mergeable'))
|
||||
Assert-Equal 1 $mergeable.RegionCount 'Events covered by one legal total window must merge.'
|
||||
Assert-Equal '2' $mergeable.TransitionCounts 'The merged region must retain both events.'
|
||||
|
||||
$partition = $planScenario.Invoke($null, @('ThreeEventPartition'))
|
||||
Assert-Equal 2 $partition.RegionCount 'Three events must split at the first infeasible joint window.'
|
||||
Assert-Equal '2,1' $partition.TransitionCounts 'Only a feasible consecutive subgroup may merge.'
|
||||
|
||||
$boundary = $planScenario.Invoke($null, @('NearBoundary'))
|
||||
Assert-True ($boundary.FirstRightLength -gt $boundary.FirstLeftLength) `
|
||||
'A boundary-clamped total window must transfer missing length to the available side.'
|
||||
Assert-True ($boundary.MaximumWindowLength -le 0.80 + 1e-9) `
|
||||
'No candidate window may exceed 0.80 m total length.'
|
||||
|
||||
foreach ($snapshot in @($separated, $mergeable, $partition, $boundary)) {
|
||||
Assert-True $snapshot.ExactEnvelope 'Region envelope must equal the extrema of actual legal variants.'
|
||||
Assert-True ($snapshot.MaximumWindowLength -le 0.80 + 1e-9) `
|
||||
'MaximumWindowLengthMeters is a total, not a per-side length.'
|
||||
}
|
||||
|
||||
$repeat = $planScenario.Invoke($null, @('ThreeEventPartition'))
|
||||
Assert-Equal $partition.Signature $repeat.Signature `
|
||||
'Repeated planning must preserve grouping, candidate numbering and variant order.'
|
||||
|
||||
Write-Output 'Path smoothing Local G2 detection checks passed.'
|
||||
@@ -0,0 +1,136 @@
|
||||
param(
|
||||
[string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'),
|
||||
[string]$FixturePath = (Join-Path $PSScriptRoot '..\ParkrobTrajplanner\PathSmoothing\Test\Fixtures\path-smoothing-fixtures.json')
|
||||
)
|
||||
|
||||
$ErrorActionPreference = 'Stop'
|
||||
$newtonsoft = Join-Path $env:USERPROFILE '.nuget\packages\newtonsoft.json\13.0.4\lib\netstandard2.0\Newtonsoft.Json.dll'
|
||||
if (Test-Path $newtonsoft) { $null = [Reflection.Assembly]::LoadFrom($newtonsoft) }
|
||||
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
|
||||
|
||||
function Assert-True($Actual, [string]$Message) { if (-not $Actual) { throw $Message } }
|
||||
function Assert-Equal($Expected, $Actual, [string]$Message) {
|
||||
if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" }
|
||||
}
|
||||
function Assert-Near([double]$Expected, [double]$Actual, [string]$Message) {
|
||||
if ([Math]::Abs($Expected - $Actual) -gt 0.000000001) {
|
||||
throw "$Message Expected=$Expected Actual=$Actual"
|
||||
}
|
||||
}
|
||||
function Get-RequiredType([string]$Name) { return $assembly.GetType($Name, $true) }
|
||||
|
||||
$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
|
||||
$facade = $root + 'Facade.'
|
||||
$test = $root + 'Test.'
|
||||
$serviceType = Get-RequiredType ($facade + 'PathSmoothingService')
|
||||
$requestType = Get-RequiredType ($root + 'PathSmoothingRequest')
|
||||
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
|
||||
$methodType = Get-RequiredType ($root + 'SmoothingMethod')
|
||||
$factoryType = Get-RequiredType ($test + 'SmoothingScenarioFactory')
|
||||
$localG2 = [Enum]::Parse($methodType, 'LocalG2Quintic')
|
||||
$service = [Activator]::CreateInstance($serviceType)
|
||||
$smooth = $serviceType.GetMethod('Smooth', [Type[]]@($requestType, [Threading.CancellationToken]))
|
||||
$createFixtures = $factoryType.GetMethod('CreateFixtureRequests', [Type[]]@([string]))
|
||||
Assert-True ($null -ne $smooth) 'Local G2 must use the public service entrypoint.'
|
||||
|
||||
function New-LocalG2Request($BaseRequest, [scriptblock]$Configure = $null) {
|
||||
$configuration = [Activator]::CreateInstance($configurationType)
|
||||
$configuration.Method = $localG2
|
||||
$configuration.AllowFallbackToCoarsePath = $true
|
||||
if ($null -ne $Configure) { & $Configure $configuration }
|
||||
return [Activator]::CreateInstance($requestType, @(
|
||||
$BaseRequest.CoarsePath, $BaseRequest.Segments, $BaseRequest.Map,
|
||||
$BaseRequest.Vehicle, $configuration))
|
||||
}
|
||||
|
||||
function Invoke-LocalG2($Request, [Threading.CancellationToken]$CancellationToken = [Threading.CancellationToken]::None) {
|
||||
return $smooth.Invoke($service, @($Request, $CancellationToken))
|
||||
}
|
||||
|
||||
function Assert-PublishedPath($Result, [string]$Name) {
|
||||
Assert-True ($Result.Path.Count -gt 0) "$Name must publish a complete path."
|
||||
Assert-Equal 0 $Result.Segments[0].StartIndex "$Name segments must start at zero."
|
||||
Assert-Equal ($Result.Path.Count - 1) $Result.Segments[-1].EndIndex "$Name segments must cover the path."
|
||||
Assert-True $Result.Diagnostics.Metrics.IsFeasible "$Name diagnostics must be feasible."
|
||||
Assert-True ($Result.Diagnostics.Metrics.MinimumBodyClearanceMeters -ge 0.02) "$Name must retain the configured clearance reserve."
|
||||
}
|
||||
|
||||
function Assert-Equivalent($First, $Second, [string]$Name) {
|
||||
Assert-Equal $First.Status $Second.Status "$Name must retain deterministic status."
|
||||
Assert-Equal $First.Path.Count $Second.Path.Count "$Name must retain deterministic point count."
|
||||
Assert-Equal $First.RegionReports.Count $Second.RegionReports.Count "$Name must retain deterministic report count."
|
||||
for ($index = 0; $index -lt $First.Path.Count; $index++) {
|
||||
Assert-Near $First.Path[$index].X $Second.Path[$index].X "$Name point $index must retain X."
|
||||
Assert-Near $First.Path[$index].Y $Second.Path[$index].Y "$Name point $index must retain Y."
|
||||
}
|
||||
for ($index = 0; $index -lt $First.RegionReports.Count; $index++) {
|
||||
Assert-Equal $First.RegionReports[$index].SelectedCandidateIndex $Second.RegionReports[$index].SelectedCandidateIndex "$Name report $index must retain candidate selection."
|
||||
Assert-Equal $First.RegionReports[$index].Status $Second.RegionReports[$index].Status "$Name report $index must retain status."
|
||||
Assert-Near $First.RegionReports[$index].StartArcLengthMeters $Second.RegionReports[$index].StartArcLengthMeters "$Name report $index must retain order."
|
||||
}
|
||||
}
|
||||
|
||||
$fixtures = $createFixtures.Invoke($null, @((Resolve-Path $FixturePath).Path))
|
||||
$expected = @{
|
||||
'straight' = @('NotNeeded')
|
||||
'single-turn' = @('Complete')
|
||||
'large-heading-change' = @('Complete')
|
||||
's-bend' = @('Complete', 'PartialImprovement')
|
||||
'rectangle-detour' = @('Complete', 'PartialImprovement')
|
||||
'multi-obstacle-detour' = @('Complete', 'PartialImprovement')
|
||||
'forward-reverse-switch' = @('Complete', 'PartialImprovement', 'NotNeeded')
|
||||
}
|
||||
|
||||
$results = @{}
|
||||
foreach ($fixture in $fixtures) {
|
||||
$request = New-LocalG2Request $fixture.SmoothingRequest
|
||||
$first = Invoke-LocalG2 $request
|
||||
$second = Invoke-LocalG2 $request
|
||||
$scenario = $fixture.SmoothingRequest.CoarsePath[0].Source.ToString() # fixture index is resolved below by stable input order
|
||||
$results[$results.Count] = $first
|
||||
Write-Output ("Fixture $($results.Count - 1): status=$($first.Status); reports=$($first.RegionReports.Count); reason=$($first.Diagnostics.Reason)")
|
||||
foreach ($report in $first.RegionReports) {
|
||||
Write-Output (" region=$($report.Status)/$($report.FailureReason); candidates=$($report.CandidateCount); selected=$($report.SelectedCandidateIndex)")
|
||||
}
|
||||
Assert-Equivalent $first $second "Fixture $($results.Count - 1)"
|
||||
}
|
||||
|
||||
$fixtureIds = @('straight', 'single-turn', 's-bend', 'large-heading-change', 'rectangle-detour', 'multi-obstacle-detour', 'narrow-corridor', 'forward-reverse-switch')
|
||||
for ($index = 0; $index -lt $fixtureIds.Count; $index++) {
|
||||
$id = $fixtureIds[$index]
|
||||
if (-not $expected.ContainsKey($id)) { continue }
|
||||
$result = $results[$index]
|
||||
Assert-True ($expected[$id] -contains $result.Status.ToString()) "$id must publish its required Local G2 status."
|
||||
Assert-PublishedPath $result $id
|
||||
if ($result.Status.ToString() -in @('Complete', 'PartialImprovement')) {
|
||||
Assert-True (@($result.RegionReports | Where-Object { $_.Status.ToString() -eq 'Improved' }).Count -gt 0) "$id must report an accepted region."
|
||||
}
|
||||
if ($result.Status.ToString() -eq 'PartialImprovement') {
|
||||
Assert-True (@($result.RegionReports | Where-Object { $_.Status.ToString() -eq 'RetainedOriginal' }).Count -gt 0) "$id must report retained regions."
|
||||
}
|
||||
}
|
||||
|
||||
# The multi-obstacle fixture is a same-direction two-window integration case. Its two replacements
|
||||
# change length, so processing it front-to-back would invalidate the later original arc window.
|
||||
$twoRegion = $results[5]
|
||||
Assert-True ($twoRegion.RegionReports.Count -ge 2) 'Two-region integration fixture must publish both detector regions.'
|
||||
Assert-Equal 'Improved' $twoRegion.RegionReports[0].Status.ToString() 'First detector-order two-region report must improve.'
|
||||
Assert-Equal 'Improved' $twoRegion.RegionReports[1].Status.ToString() 'Second detector-order two-region report must improve.'
|
||||
Assert-True ($twoRegion.RegionReports[0].StartArcLengthMeters -lt $twoRegion.RegionReports[1].StartArcLengthMeters) 'Reports must retain detector ascending arc order.'
|
||||
Assert-True (@($twoRegion.Path | Where-Object { $_.Source.ToString() -eq 'LocalG2Transition' }).Count -gt 2) 'Both Local G2 replacements must remain in the final path.'
|
||||
|
||||
$strictRequest = New-LocalG2Request $fixtures[1].SmoothingRequest {
|
||||
param($configuration)
|
||||
$configuration.LocalG2Quintic.MinimumPeakGradientImprovementRatio = 0.99
|
||||
}
|
||||
$unchanged = Invoke-LocalG2 $strictRequest
|
||||
Assert-Equal 'Unchanged' $unchanged.Status.ToString() 'Detected events with no accepted region must publish a verified raw path as Unchanged.'
|
||||
Assert-PublishedPath $unchanged 'Unchanged Local G2'
|
||||
|
||||
$cancelSource = [Threading.CancellationTokenSource]::new()
|
||||
$cancelSource.Cancel()
|
||||
$cancelled = Invoke-LocalG2 (New-LocalG2Request $fixtures[1].SmoothingRequest) $cancelSource.Token
|
||||
Assert-Equal 'Cancelled' $cancelled.Status.ToString() 'Cancellation must propagate through Local G2.'
|
||||
Assert-Equal 0 $cancelled.Path.Count 'A cancelled run must not publish a partial path.'
|
||||
|
||||
Write-Output 'Local G2 service integration checks passed.'
|
||||
@@ -0,0 +1,18 @@
|
||||
param(
|
||||
[string]$FixturePath = (Join-Path $PSScriptRoot '..\ParkrobTrajplanner\PathSmoothing\Test\Fixtures\path-smoothing-fixtures.json')
|
||||
)
|
||||
|
||||
$ErrorActionPreference = 'Stop'
|
||||
|
||||
$hostProject = Join-Path $PSScriptRoot 'PathSmoothingPngVerificationHost\PathSmoothingPngVerificationHost.csproj'
|
||||
if (-not (Test-Path -LiteralPath $hostProject)) {
|
||||
throw "Path smoothing PNG verification host was not found: $hostProject"
|
||||
}
|
||||
|
||||
$resolvedFixturePath = (Resolve-Path -LiteralPath $FixturePath).Path
|
||||
& dotnet run --project $hostProject --no-restore -- $resolvedFixturePath
|
||||
if ($LASTEXITCODE -ne 0) {
|
||||
exit $LASTEXITCODE
|
||||
}
|
||||
|
||||
Write-Output 'Path smoothing PNG checks passed.'
|
||||
@@ -0,0 +1,338 @@
|
||||
param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
|
||||
|
||||
$ErrorActionPreference = 'Stop'
|
||||
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
|
||||
|
||||
function Assert-True($Actual, [string]$Message) {
|
||||
if (-not $Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-Equal($Expected, $Actual, [string]$Message) {
|
||||
if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" }
|
||||
}
|
||||
|
||||
function Assert-Near([double]$Expected, [double]$Actual, [double]$Tolerance, [string]$Message) {
|
||||
if ([Math]::Abs($Expected - $Actual) -gt $Tolerance) {
|
||||
throw "$Message Expected=$Expected Actual=$Actual Tolerance=$Tolerance"
|
||||
}
|
||||
}
|
||||
|
||||
function Get-RequiredType([string]$Name) {
|
||||
return $assembly.GetType($Name, $true)
|
||||
}
|
||||
|
||||
function Get-PropertyValue($Instance, [string]$Name) {
|
||||
$property = $Instance.GetType().GetProperty($Name, [Reflection.BindingFlags]'Instance,Public,NonPublic')
|
||||
Assert-True ($null -ne $property) ("Missing property: " + $Name)
|
||||
return $property.GetValue($Instance)
|
||||
}
|
||||
|
||||
function New-Point(
|
||||
[double]$X,
|
||||
[double]$Y,
|
||||
[double]$ArcLength,
|
||||
[double]$Heading,
|
||||
[double]$BodyClearance = 1.0,
|
||||
[bool]$IsGearSwitch = $false) {
|
||||
return [Activator]::CreateInstance($pointType, @(
|
||||
$X, $Y, $ArcLength, $Heading, $Heading, $BodyClearance, $IsGearSwitch, $anchor))
|
||||
}
|
||||
|
||||
function New-DirectionSegment(
|
||||
[int]$Index,
|
||||
$Direction,
|
||||
[object[]]$Points,
|
||||
[bool]$StartsAtGearSwitch = $false,
|
||||
[bool]$EndsAtGearSwitch = $false) {
|
||||
$typedPoints = [Array]::CreateInstance($pointType, $Points.Count)
|
||||
for ($pointIndex = 0; $pointIndex -lt $Points.Count; $pointIndex++) {
|
||||
$typedPoints.SetValue($Points[$pointIndex], $pointIndex)
|
||||
}
|
||||
return [Activator]::CreateInstance($segmentType, @(
|
||||
$Index, $Direction, $typedPoints, $StartsAtGearSwitch, $EndsAtGearSwitch))
|
||||
}
|
||||
|
||||
function New-EmptyMap {
|
||||
$request = [Activator]::CreateInstance($mapRequestType)
|
||||
$request.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000))
|
||||
$request.ResolutionMm = [single]50
|
||||
$request.AllowExplicitEmptyMap = $true
|
||||
$map = [Activator]::CreateInstance($mapFactoryType).Create($request).Map
|
||||
Assert-True ($null -ne $map) 'Quintic test must create an explicit empty planning map.'
|
||||
return $map
|
||||
}
|
||||
|
||||
function New-AlgorithmInput(
|
||||
[object[]]$Segments,
|
||||
[double]$ReserveMeters,
|
||||
[double]$KnotSpacingMeters = 1.0,
|
||||
[double]$MinimumKnotSpacingMeters = 0.10) {
|
||||
$typedSegments = [Array]::CreateInstance($segmentType, $Segments.Count)
|
||||
for ($index = 0; $index -lt $Segments.Count; $index++) {
|
||||
$typedSegments.SetValue($Segments[$index], $index)
|
||||
}
|
||||
$preparedPath = [Activator]::CreateInstance($preparedPathType, [object[]]@(,$typedSegments))
|
||||
$vehicle = [Activator]::CreateInstance($vehicleType)
|
||||
$vehicle.LengthMeters = [double]0.20
|
||||
$vehicle.WidthMeters = [double]0.20
|
||||
$vehicle.SafetyMarginMeters = [double]0.0
|
||||
$vehicle.MaximumCurvaturePerMeter = [double]100.0
|
||||
$vehicle.MinimumTurningRadiusMeters = [double]0.01
|
||||
$configuration = [Activator]::CreateInstance($configurationType)
|
||||
$configuration.PiecewiseQuintic.KnotSpacingMeters = $KnotSpacingMeters
|
||||
$configuration.PiecewiseQuintic.MinimumKnotSpacingMeters = $MinimumKnotSpacingMeters
|
||||
$options = $optionsConstructor.Invoke(@($configuration))
|
||||
return $inputConstructor.Invoke(@($preparedPath, (New-EmptyMap), $vehicle, [double]0.05, $ReserveMeters, $options))
|
||||
}
|
||||
|
||||
function Invoke-Candidate(
|
||||
[object[]]$Segments,
|
||||
[double]$ReserveMeters = 0.0,
|
||||
[double]$KnotSpacingMeters = 1.0,
|
||||
[double]$MinimumKnotSpacingMeters = 0.10) {
|
||||
return $smoothMethod.Invoke($smoother, @(
|
||||
(New-AlgorithmInput $Segments $ReserveMeters $KnotSpacingMeters $MinimumKnotSpacingMeters),
|
||||
[double]1.0, [Threading.CancellationToken]::None))
|
||||
}
|
||||
|
||||
function Invoke-Smoothing(
|
||||
[object[]]$Segments,
|
||||
[double]$ReserveMeters = 0.0,
|
||||
[double]$KnotSpacingMeters = 1.0,
|
||||
[double]$MinimumKnotSpacingMeters = 0.10) {
|
||||
$candidate = Invoke-Candidate $Segments $ReserveMeters $KnotSpacingMeters $MinimumKnotSpacingMeters
|
||||
Assert-True (Get-PropertyValue $candidate 'Succeeded') 'Quintic smoothing must produce a candidate for the deterministic fixture.'
|
||||
return @(Get-PropertyValue $candidate 'Segments')
|
||||
}
|
||||
|
||||
function Get-PointDistance($Left, $Right) {
|
||||
$deltaX = $Left.X - $Right.X
|
||||
$deltaY = $Left.Y - $Right.Y
|
||||
return [Math]::Sqrt($deltaX * $deltaX + $deltaY * $deltaY)
|
||||
}
|
||||
|
||||
function Get-Reference([object[]]$Source, [double]$ArcLength) {
|
||||
$typedPoints = [Array]::CreateInstance($pointType, $Source.Count)
|
||||
for ($index = 0; $index -lt $Source.Count; $index++) { $typedPoints.SetValue($Source[$index], $index) }
|
||||
$arguments = [object[]]@($typedPoints, $ArcLength, $null, $null)
|
||||
Assert-True $interpolateMethod.Invoke($null, $arguments) 'Quintic test must resolve every sampled local-arc reference.'
|
||||
return $arguments[2]
|
||||
}
|
||||
|
||||
function Get-PointAtArcLength([object[]]$Points, [double]$ArcLength) {
|
||||
foreach ($point in $Points) {
|
||||
if ([Math]::Abs($point.ArcLength - $ArcLength) -lt 0.000000000001) { return $point }
|
||||
}
|
||||
throw "Missing quintic sample at arc length $ArcLength"
|
||||
}
|
||||
|
||||
function Get-EndpointDerivative([object[]]$Samples, [double]$StepMeters, [bool]$AtStart) {
|
||||
$firstCoefficients = @((-137.0 / 60.0), 5.0, -5.0, (10.0 / 3.0), (-5.0 / 4.0), (1.0 / 5.0))
|
||||
$x = 0.0
|
||||
$y = 0.0
|
||||
for ($index = 0; $index -lt 6; $index++) {
|
||||
$sampleIndex = if ($AtStart) { $index } else { 5 - $index }
|
||||
$sign = if ($AtStart) { 1.0 } else { -1.0 }
|
||||
$x += $firstCoefficients[$index] * $Samples[$sampleIndex].X
|
||||
$y += $firstCoefficients[$index] * $Samples[$sampleIndex].Y
|
||||
}
|
||||
return [PSCustomObject]@{ X = $sign * $x / $StepMeters; Y = $sign * $y / $StepMeters }
|
||||
}
|
||||
|
||||
function Get-EndpointSecondDerivative([object[]]$Samples, [double]$StepMeters, [bool]$AtStart) {
|
||||
$coefficients = @((15.0 / 4.0), (-77.0 / 6.0), (107.0 / 6.0), -13.0, (61.0 / 12.0), (-5.0 / 6.0))
|
||||
$x = 0.0
|
||||
$y = 0.0
|
||||
for ($index = 0; $index -lt 6; $index++) {
|
||||
$sampleIndex = if ($AtStart) { $index } else { 5 - $index }
|
||||
$x += $coefficients[$index] * $Samples[$sampleIndex].X
|
||||
$y += $coefficients[$index] * $Samples[$sampleIndex].Y
|
||||
}
|
||||
return [PSCustomObject]@{ X = $x / ($StepMeters * $StepMeters); Y = $y / ($StepMeters * $StepMeters) }
|
||||
}
|
||||
|
||||
function Get-IntervalSamples([object[]]$Points, [double]$StartArcLength, [double]$EndArcLength, [bool]$FromStart) {
|
||||
$intervalLength = $EndArcLength - $StartArcLength
|
||||
$result = @()
|
||||
for ($index = 0; $index -lt 6; $index++) {
|
||||
$parameter = if ($FromStart) { $index / 8.0 } else { (3.0 + $index) / 8.0 }
|
||||
$result += Get-PointAtArcLength $Points ($StartArcLength + $parameter * $intervalLength)
|
||||
}
|
||||
return $result
|
||||
}
|
||||
|
||||
function Get-QuinticPositionFromInteriorSamples(
|
||||
[object[]]$Points,
|
||||
[double]$StartArcLength,
|
||||
[double]$EndArcLength,
|
||||
[double[]]$Parameters,
|
||||
[double]$TargetParameter) {
|
||||
$intervalLength = $EndArcLength - $StartArcLength
|
||||
$x = 0.0
|
||||
$y = 0.0
|
||||
for ($index = 0; $index -lt $Parameters.Count; $index++) {
|
||||
$weight = 1.0
|
||||
for ($otherIndex = 0; $otherIndex -lt $Parameters.Count; $otherIndex++) {
|
||||
if ($index -ne $otherIndex) {
|
||||
$weight *= ($TargetParameter - $Parameters[$otherIndex]) / ($Parameters[$index] - $Parameters[$otherIndex])
|
||||
}
|
||||
}
|
||||
$sample = Get-PointAtArcLength $Points ($StartArcLength + $Parameters[$index] * $intervalLength)
|
||||
$x += $weight * $sample.X
|
||||
$y += $weight * $sample.Y
|
||||
}
|
||||
return [PSCustomObject]@{ X = $x; Y = $y }
|
||||
}
|
||||
|
||||
$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
|
||||
$processing = $root + 'Processing.'
|
||||
$algorithms = $root + 'Algorithms.'
|
||||
$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
|
||||
|
||||
$smootherType = Get-RequiredType ($algorithms + 'PiecewiseQuinticSmoother')
|
||||
$pointType = Get-RequiredType ($processing + 'SmoothingPoint2D')
|
||||
$segmentType = Get-RequiredType ($processing + 'PreparedDirectionSegment')
|
||||
$preparedPathType = Get-RequiredType ($processing + 'PreparedPath')
|
||||
$inputType = Get-RequiredType ($algorithms + 'SmoothingAlgorithmInput')
|
||||
$optionsType = Get-RequiredType ($algorithms + 'SmoothingOptionsSnapshot')
|
||||
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
|
||||
$interpolatorType = Get-RequiredType ($processing + 'PathReferenceInterpolator')
|
||||
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
|
||||
$directionType = Get-RequiredType ($coarsePath + 'TravelDirection')
|
||||
$sourceType = Get-RequiredType ($root + 'SmoothedPathPointSource')
|
||||
$boundsType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.MapBoundsMm'
|
||||
$mapType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningGridMap'
|
||||
$mapRequestType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapRequest'
|
||||
$mapFactoryType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapFactory'
|
||||
|
||||
$inputConstructor = $inputType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null,
|
||||
@($preparedPathType, $mapType, $vehicleType, [double], [double], $optionsType), $null)
|
||||
Assert-True ($null -ne $inputConstructor) 'Algorithm input must carry immutable quintic options and clearance reserve.'
|
||||
$optionsConstructor = $optionsType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null, @($configurationType), $null)
|
||||
Assert-True ($null -ne $optionsConstructor) 'Quintic tests must create immutable options snapshots.'
|
||||
$interpolateMethod = $interpolatorType.GetMethod('TryInterpolateByArcLength', [Reflection.BindingFlags]'Static,Public,NonPublic')
|
||||
Assert-True ($null -ne $interpolateMethod) 'PathReferenceInterpolator must expose local-arc interpolation.'
|
||||
$smoother = [Activator]::CreateInstance($smootherType, $true)
|
||||
$smoothMethod = $smootherType.GetMethod('Smooth', [Reflection.BindingFlags]'Instance,Public')
|
||||
Assert-True ($null -ne $smoothMethod) 'PiecewiseQuinticSmoother must implement the internal smoother contract.'
|
||||
Assert-Equal 'PiecewiseQuintic' $smoother.Method.ToString() 'Quintic smoother must identify its public smoothing method.'
|
||||
|
||||
$forward = [Enum]::Parse($directionType, 'Forward')
|
||||
$reverse = [Enum]::Parse($directionType, 'Reverse')
|
||||
$anchor = [Enum]::Parse($sourceType, 'Anchor')
|
||||
|
||||
# The 1.0 m local-arc knot spacing creates shared knots at s=1 and s=2.
|
||||
# The generated 1/8 samples allow exact one-sided quintic derivative reconstruction.
|
||||
$continuitySource = @(
|
||||
(New-Point 0.00 0.00 0.00 0.00),
|
||||
(New-Point 0.50 0.00 0.50 0.00),
|
||||
(New-Point 1.00 0.00 1.00 0.00),
|
||||
(New-Point 1.00 0.50 1.50 ([Math]::PI / 2.0)),
|
||||
(New-Point 1.00 1.00 2.00 ([Math]::PI / 2.0)),
|
||||
(New-Point 1.30 1.30 2.40 ([Math]::PI / 4.0)))
|
||||
$continuityOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $continuitySource)))[0].Points
|
||||
foreach ($sharedArcLength in @(1.0, 2.0)) {
|
||||
$leftSamples = Get-IntervalSamples $continuityOutput ($sharedArcLength - 1.0) $sharedArcLength $false
|
||||
$rightEnd = if ($sharedArcLength -eq 2.0) { 2.4 } else { $sharedArcLength + 1.0 }
|
||||
$rightSamples = Get-IntervalSamples $continuityOutput $sharedArcLength $rightEnd $true
|
||||
$leftPosition = Get-QuinticPositionFromInteriorSamples $continuityOutput ($sharedArcLength - 1.0) $sharedArcLength `
|
||||
@((2.0 / 8.0), (3.0 / 8.0), (4.0 / 8.0), (5.0 / 8.0), (6.0 / 8.0), (7.0 / 8.0)) 1.0
|
||||
$rightPosition = Get-QuinticPositionFromInteriorSamples $continuityOutput $sharedArcLength $rightEnd `
|
||||
@((1.0 / 8.0), (2.0 / 8.0), (3.0 / 8.0), (4.0 / 8.0), (5.0 / 8.0), (6.0 / 8.0)) 0.0
|
||||
$leftFirst = Get-EndpointDerivative $leftSamples (1.0 / 8.0) $false
|
||||
$rightFirst = Get-EndpointDerivative $rightSamples (($rightEnd - $sharedArcLength) / 8.0) $true
|
||||
$leftSecond = Get-EndpointSecondDerivative $leftSamples (1.0 / 8.0) $false
|
||||
$rightSecond = Get-EndpointSecondDerivative $rightSamples (($rightEnd - $sharedArcLength) / 8.0) $true
|
||||
Assert-Near $leftPosition.X $rightPosition.X 0.000001 'Shared knot X position must match from both quintic intervals.'
|
||||
Assert-Near $leftPosition.Y $rightPosition.Y 0.000001 'Shared knot Y position must match from both quintic intervals.'
|
||||
Assert-Near $leftFirst.X $rightFirst.X 0.000001 'Shared knot X first derivative must be C1.'
|
||||
Assert-Near $leftFirst.Y $rightFirst.Y 0.000001 'Shared knot Y first derivative must be C1.'
|
||||
Assert-Near $leftSecond.X $rightSecond.X 0.000001 'Shared knot X second derivative must be C2.'
|
||||
Assert-Near $leftSecond.Y $rightSecond.Y 0.000001 'Shared knot Y second derivative must be C2.'
|
||||
}
|
||||
|
||||
$firstOutput = $continuityOutput[0]
|
||||
$lastOutput = $continuityOutput[$continuityOutput.Count - 1]
|
||||
Assert-Near $continuitySource[0].X $firstOutput.X 0.0 'Quintic start X must remain exact.'
|
||||
Assert-Near $continuitySource[0].Y $firstOutput.Y 0.0 'Quintic start Y must remain exact.'
|
||||
Assert-Near $continuitySource[$continuitySource.Count - 1].X $lastOutput.X 0.0 'Quintic end X must remain exact.'
|
||||
Assert-Near $continuitySource[$continuitySource.Count - 1].Y $lastOutput.Y 0.0 'Quintic end Y must remain exact.'
|
||||
foreach ($point in $continuityOutput) {
|
||||
$reference = Get-Reference $continuitySource $point.ArcLength
|
||||
Assert-True ((Get-PointDistance $point $reference) -le ($reference.BodyClearance + 0.000000000001)) 'Every quintic sample must remain inside its local reference movement bound.'
|
||||
}
|
||||
|
||||
# Separate prepared direction segments must retain their exact duplicated switch pose and topology.
|
||||
$reverseSource = @(
|
||||
(New-Point 1.30 1.30 0.00 ([Math]::PI / 4.0) 1.0 $true),
|
||||
(New-Point 1.30 0.80 0.50 ([Math]::PI / 2.0)),
|
||||
(New-Point 1.30 0.30 1.00 ([Math]::PI / 2.0)))
|
||||
$switchOutput = @(Invoke-Smoothing @(
|
||||
(New-DirectionSegment 0 $forward $continuitySource $false $true),
|
||||
(New-DirectionSegment 1 $reverse $reverseSource $true $false)))
|
||||
Assert-Equal 2 $switchOutput.Count 'Quintic smoothing must retain separate direction segments.'
|
||||
Assert-True $switchOutput[0].EndsAtGearSwitch 'Forward quintic segment must retain its gear-switch boundary flag.'
|
||||
Assert-True $switchOutput[1].StartsAtGearSwitch 'Reverse quintic segment must retain its gear-switch boundary flag.'
|
||||
$leftSwitch = $switchOutput[0].Points[$switchOutput[0].Points.Count - 1]
|
||||
$rightSwitch = $switchOutput[1].Points[0]
|
||||
Assert-Near $leftSwitch.X $rightSwitch.X 0.0 'Quintic smoothing must preserve switch X exactly.'
|
||||
Assert-Near $leftSwitch.Y $rightSwitch.Y 0.0 'Quintic smoothing must preserve switch Y exactly.'
|
||||
|
||||
# Spacing selects local-arc knots, while a valid minimum spacing does not change that selection.
|
||||
$oneMeterOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $continuitySource)) 0.0 1.0 0.10)[0].Points
|
||||
$halfMeterOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $continuitySource)) 0.0 0.50 0.10)[0].Points
|
||||
$largeMinimumOutput = @(Invoke-Smoothing @((New-DirectionSegment 0 $forward $continuitySource)) 0.0 1.0 0.30)[0].Points
|
||||
Assert-True ($halfMeterOutput.Count -gt $oneMeterOutput.Count) 'Custom knot spacing must create additional local-arc knot intervals.'
|
||||
Assert-Equal $oneMeterOutput.Count $largeMinimumOutput.Count 'Valid minimum knot spacing must not change knot selection.'
|
||||
for ($index = 0; $index -lt $oneMeterOutput.Count; $index++) {
|
||||
Assert-Near $oneMeterOutput[$index].X $largeMinimumOutput[$index].X 0.000000000001 'Minimum knot spacing must not change valid quintic X geometry.'
|
||||
Assert-Near $oneMeterOutput[$index].Y $largeMinimumOutput[$index].Y 0.000000000001 'Minimum knot spacing must not change valid quintic Y geometry.'
|
||||
}
|
||||
|
||||
$shortSource = @(
|
||||
(New-Point 0.00 0.00 0.00 0.00),
|
||||
(New-Point 0.05 0.00 0.05 0.00))
|
||||
$shortCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $shortSource)) 0.0 1.0 0.10
|
||||
Assert-Equal 'Failed' (Get-PropertyValue $shortCandidate 'Status').ToString() 'A segment shorter than the configured minimum knot spacing must fail terminally.'
|
||||
Assert-True (-not (Get-PropertyValue $shortCandidate 'Succeeded')) 'A degenerate short quintic segment must not be executable.'
|
||||
Assert-Equal 0 (Get-PropertyValue $shortCandidate 'Segments').Count 'A terminal quintic degeneracy must publish no geometry.'
|
||||
|
||||
# Repeated decimal addition must not create a spurious 1e-16 m terminal residual when the
|
||||
# requested spacing divides the local length exactly. A real 0.005 m remainder remains below
|
||||
# the 0.01 m minimum and must still fail terminally.
|
||||
$decimalMultipleSource = @(
|
||||
(New-Point 0.0 0.0 0.0 0.0),
|
||||
(New-Point 1.0 0.0 1.0 0.0))
|
||||
$decimalMultipleCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $decimalMultipleSource)) 0.0 0.10 0.01
|
||||
Assert-Equal 'Success' (Get-PropertyValue $decimalMultipleCandidate 'Status').ToString() 'Decimal-exact knot multiples must not become a terminal under-minimum residual failure.'
|
||||
$decimalMultipleOutput = @(Get-PropertyValue $decimalMultipleCandidate 'Segments')[0].Points
|
||||
Assert-Equal 81 $decimalMultipleOutput.Count 'A 1.0 m segment with 0.1 m spacing must create exactly ten valid quintic intervals.'
|
||||
Assert-Near 1.0 $decimalMultipleOutput[$decimalMultipleOutput.Count - 1].ArcLength 0.0 'Decimal-multiple knot normalization must retain the exact terminal arc length.'
|
||||
|
||||
$meaningfulShortResidualSource = @(
|
||||
(New-Point 0.0 0.0 0.0 0.0),
|
||||
(New-Point 1.005 0.0 1.005 0.0))
|
||||
$meaningfulShortResidualCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $meaningfulShortResidualSource)) 0.0 0.10 0.01
|
||||
Assert-Equal 'Failed' (Get-PropertyValue $meaningfulShortResidualCandidate 'Status').ToString() 'A genuine 0.005 m terminal residual must remain a terminal spacing failure.'
|
||||
Assert-Equal 0 (Get-PropertyValue $meaningfulShortResidualCandidate 'Segments').Count 'A genuine under-minimum residual must publish no geometry.'
|
||||
|
||||
$smallScaleResidualSource = @(
|
||||
(New-Point 0.0 0.0 0.0 0.0),
|
||||
(New-Point 0.000000000000105 0.0 0.000000000000105 0.0))
|
||||
$smallScaleResidualCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $smallScaleResidualSource)) 0.0 0.00000000000001 0.000000000000006
|
||||
Assert-Equal 'Failed' (Get-PropertyValue $smallScaleResidualCandidate 'Status').ToString() 'Endpoint normalization tolerance must scale with local arc magnitude and preserve a genuine small residual failure.'
|
||||
|
||||
# Local-arc reference mapping, rather than sample index/global distance, must reject this unsafe nonuniform path.
|
||||
$nonuniformUnsafeSource = @(
|
||||
(New-Point 0.0 0.0 0.0 0.0 0.50),
|
||||
(New-Point 1.0 0.0 4.0 0.0 0.50),
|
||||
(New-Point 2.0 0.0 5.0 0.0 0.50),
|
||||
(New-Point 3.0 0.0 6.0 0.0 0.50),
|
||||
(New-Point 3.0 1.0 9.0 ([Math]::PI / 2.0) 0.50),
|
||||
(New-Point 3.0 2.0 20.0 ([Math]::PI / 2.0) 0.50))
|
||||
$nonuniformUnsafeCandidate = Invoke-Candidate @((New-DirectionSegment 0 $forward $nonuniformUnsafeSource)) 0.0 5.0 0.10
|
||||
Assert-Equal 'RetryableInfeasible' (Get-PropertyValue $nonuniformUnsafeCandidate 'Status').ToString() 'Unsafe nonuniform local-arc quintic movement must be retryable.'
|
||||
Assert-True (-not (Get-PropertyValue $nonuniformUnsafeCandidate 'Succeeded')) 'Unsafe nonuniform quintic geometry must not be executable.'
|
||||
Assert-Equal 0 (Get-PropertyValue $nonuniformUnsafeCandidate 'Segments').Count 'Retryable quintic infeasibility must publish no geometry.'
|
||||
|
||||
Write-Output 'Path smoothing piecewise quintic checks passed.'
|
||||
@@ -0,0 +1,81 @@
|
||||
param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
|
||||
|
||||
$ErrorActionPreference = 'Stop'
|
||||
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
|
||||
|
||||
function Assert-True($Actual, [string]$Message) {
|
||||
if (-not $Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-False($Actual, [string]$Message) {
|
||||
if ($Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-Equal($Expected, $Actual, [string]$Message) {
|
||||
if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" }
|
||||
}
|
||||
|
||||
function Assert-Near([double]$Expected, [double]$Actual, [string]$Message) {
|
||||
if ([Math]::Abs($Expected - $Actual) -gt 0.000000001) {
|
||||
throw "$Message Expected=$Expected Actual=$Actual"
|
||||
}
|
||||
}
|
||||
|
||||
function Get-RequiredType([string]$Name) {
|
||||
return $assembly.GetType($Name, $true)
|
||||
}
|
||||
|
||||
function Assert-AttemptedStrengths($Snapshot, [double[]]$Expected, [string]$Message) {
|
||||
Assert-Equal $Expected.Length $Snapshot.AttemptedStrengths.Count ($Message + ' count')
|
||||
for ($index = 0; $index -lt $Expected.Length; $index++) {
|
||||
Assert-Near $Expected[$index] $Snapshot.AttemptedStrengths[$index] ($Message + " index=$index")
|
||||
}
|
||||
}
|
||||
|
||||
$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
|
||||
$algorithms = $root + 'Algorithms.'
|
||||
$runnerType = Get-RequiredType ($algorithms + 'SmoothingAlgorithmRunner')
|
||||
$smootherType = Get-RequiredType ($algorithms + 'IPathSmoother')
|
||||
$candidateType = Get-RequiredType ($algorithms + 'SmoothingCandidate')
|
||||
$candidateStatusType = Get-RequiredType ($algorithms + 'SmoothingCandidateStatus')
|
||||
Assert-False $smootherType.IsPublic 'IPathSmoother must remain internal to the algorithm assembly.'
|
||||
Assert-Equal 3 ([Enum]::GetNames($candidateStatusType).Length) 'Smoothing candidate status must contain only the three defined feasibility states.'
|
||||
Assert-Equal 'Success' ([Enum]::GetNames($candidateStatusType)[0]) 'Candidate status must expose Success.'
|
||||
Assert-Equal 'RetryableInfeasible' ([Enum]::GetNames($candidateStatusType)[1]) 'Candidate status must expose RetryableInfeasible.'
|
||||
Assert-Equal 'Failed' ([Enum]::GetNames($candidateStatusType)[2]) 'Candidate status must expose Failed.'
|
||||
$retryableFactory = $candidateType.GetMethod('RetryableInfeasible', [Reflection.BindingFlags]'Static,NonPublic')
|
||||
Assert-True ($null -ne $retryableFactory) 'SmoothingCandidate must create retryable infeasibility without executable geometry.'
|
||||
|
||||
$hooksType = $runnerType.GetNestedType('TestHooks', [Reflection.BindingFlags]'Public,NonPublic')
|
||||
Assert-True ($null -ne $hooksType) 'SmoothingAlgorithmRunner must expose its narrowly scoped nested TestHooks helper.'
|
||||
$executeMethod = $hooksType.GetMethod('Execute', [Reflection.BindingFlags]'Public,Static')
|
||||
Assert-True ($null -ne $executeMethod) 'TestHooks must expose deterministic scenario execution for reflection tests.'
|
||||
|
||||
function Invoke-Scenario([string]$Scenario) {
|
||||
return $executeMethod.Invoke($null, @($Scenario))
|
||||
}
|
||||
|
||||
$allRetryable = Invoke-Scenario 'RetryableInfeasible'
|
||||
Assert-Equal 'Infeasible' $allRetryable.Status 'Exhausted retryable infeasibility must produce an Infeasible runner result.'
|
||||
Assert-AttemptedStrengths $allRetryable @(1.00, 0.75, 0.50, 0.25) 'Retryable infeasibility must use the finite retry schedule exactly.'
|
||||
Assert-Equal 0 $allRetryable.AcceptedPathPointCount 'An infeasible runner result must not retain a retryable candidate as an accepted path.'
|
||||
Assert-Equal 0 $allRetryable.RejectedComparisonCandidatePointCount 'Retryable infeasibility must not retain executable candidate geometry.'
|
||||
Assert-Equal 4 $allRetryable.FailureCount 'Every retryable attempt must retain its failure reason.'
|
||||
|
||||
$accepted = Invoke-Scenario 'AcceptFirst'
|
||||
Assert-Equal 'Success' $accepted.Status 'The first safe candidate must be accepted.'
|
||||
Assert-AttemptedStrengths $accepted @(1.00) 'The runner must stop immediately after the first accepted candidate.'
|
||||
Assert-True ($accepted.AcceptedPathPointCount -gt 0) 'A successful runner result must publish the validated path internally.'
|
||||
Assert-Equal 0 $accepted.RejectedComparisonCandidatePointCount 'An accepted candidate must not create rejected comparison geometry.'
|
||||
|
||||
$terminalFailure = Invoke-Scenario 'TerminalFailed'
|
||||
Assert-Equal 'Failed' $terminalFailure.Status 'A terminal candidate failure must stop the runner as Failed.'
|
||||
Assert-AttemptedStrengths $terminalFailure @(1.00) 'Terminal candidate failure must run exactly once.'
|
||||
Assert-Equal 0 $terminalFailure.RejectedComparisonCandidatePointCount 'A terminal failure must not retain comparison geometry.'
|
||||
|
||||
$cancelled = Invoke-Scenario 'CancelBeforeNextAttempt'
|
||||
Assert-True $cancelled.CancellationPropagated 'Cancellation between attempts must propagate out of the runner.'
|
||||
Assert-AttemptedStrengths $cancelled @(1.00) 'Cancellation before the next attempt must prevent another smoother call.'
|
||||
Assert-Equal 0 $cancelled.AcceptedPathPointCount 'A cancelled run must not publish a partial path.'
|
||||
|
||||
Write-Output 'Path smoothing retry runner checks passed.'
|
||||
@@ -0,0 +1,259 @@
|
||||
param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
|
||||
|
||||
$ErrorActionPreference = 'Stop'
|
||||
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
|
||||
|
||||
function Assert-True($Actual, [string]$Message) {
|
||||
if (-not $Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-Equal($Expected, $Actual, [string]$Message) {
|
||||
if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" }
|
||||
}
|
||||
|
||||
function Assert-Near([double]$Expected, [double]$Actual, [double]$Tolerance, [string]$Message) {
|
||||
if ([Math]::Abs($Expected - $Actual) -gt $Tolerance) {
|
||||
throw "$Message Expected=$Expected Actual=$Actual Tolerance=$Tolerance"
|
||||
}
|
||||
}
|
||||
|
||||
function Get-RequiredType([string]$Name) {
|
||||
return $assembly.GetType($Name, $true)
|
||||
}
|
||||
|
||||
function New-Map([bool]$WithObstacle) {
|
||||
$mapRequest = [Activator]::CreateInstance($mapRequestType)
|
||||
$mapRequest.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]5000, [single]0, [single]5000))
|
||||
$mapRequest.ResolutionMm = [single]50
|
||||
if ($WithObstacle) {
|
||||
$obstacle = [Activator]::CreateInstance($rectangleType, @([single]900, [single]1100, [single]400, [single]600))
|
||||
$obstacles = [Array]::CreateInstance($obstacleType, 1)
|
||||
$obstacles.SetValue($obstacle, 0)
|
||||
$source = [Activator]::CreateInstance($manualSourceType, @('service-safety-obstacle', [long]1, $true, $obstacles))
|
||||
$sources = [Array]::CreateInstance($obstacleSourceType, 1)
|
||||
$sources.SetValue($source, 0)
|
||||
$mapRequest.ObstacleSources = $sources
|
||||
}
|
||||
else {
|
||||
$mapRequest.AllowExplicitEmptyMap = $true
|
||||
}
|
||||
$map = [Activator]::CreateInstance($mapFactoryType).Create($mapRequest).Map
|
||||
Assert-True ($null -ne $map) 'Service test must create a planning map.'
|
||||
return $map
|
||||
}
|
||||
|
||||
function New-EmptyMap { return New-Map $false }
|
||||
|
||||
function New-CollidingMap { return New-Map $true }
|
||||
|
||||
function New-Vehicle {
|
||||
$vehicle = [Activator]::CreateInstance($vehicleType)
|
||||
$vehicle.LengthMeters = [double]0.20
|
||||
$vehicle.WidthMeters = [double]0.20
|
||||
$vehicle.SafetyMarginMeters = [double]0.0
|
||||
$vehicle.MaximumCurvaturePerMeter = [double]100.0
|
||||
return $vehicle
|
||||
}
|
||||
|
||||
function New-CoarsePoint(
|
||||
[double]$X,
|
||||
[double]$Y,
|
||||
[double]$ArcLength,
|
||||
$Direction,
|
||||
[double]$BodyClearance = 1.0,
|
||||
[bool]$IsGearSwitch = $false) {
|
||||
return [Activator]::CreateInstance($coarsePointType, @(
|
||||
$X, $Y, [double]0.0, [double]0.0, $ArcLength, $Direction,
|
||||
[double]0.0, $BodyClearance, $IsGearSwitch, $coarseAnchor))
|
||||
}
|
||||
|
||||
function New-Configuration($Method = $cubicBSpline) {
|
||||
$configuration = [Activator]::CreateInstance($configurationType)
|
||||
$configuration.Method = $Method
|
||||
return $configuration
|
||||
}
|
||||
|
||||
function New-Request([object[]]$Points, $Configuration, $Map = $null) {
|
||||
if ($null -eq $Map) { $Map = New-EmptyMap }
|
||||
$typedPoints = [Array]::CreateInstance($coarsePointType, $Points.Count)
|
||||
for ($index = 0; $index -lt $Points.Count; $index++) {
|
||||
$typedPoints.SetValue($Points[$index], $index)
|
||||
}
|
||||
|
||||
$segments = [Array]::CreateInstance($coarseSegmentType, 1)
|
||||
$segments.SetValue([Activator]::CreateInstance($coarseSegmentType, @(
|
||||
0, $forward, 0, ($Points.Count - 1), $false, $false)), 0)
|
||||
return [Activator]::CreateInstance($requestType, @($typedPoints, $segments, $Map, (New-Vehicle), $Configuration))
|
||||
}
|
||||
|
||||
function New-StraightRequest($Configuration) {
|
||||
return New-Request @(
|
||||
(New-CoarsePoint 0.5 0.5 0.0 $forward),
|
||||
(New-CoarsePoint 1.5 0.5 1.0 $forward)) $Configuration
|
||||
}
|
||||
|
||||
function New-InfeasibleRequest($Configuration) {
|
||||
# Zero declared movement clearance makes every non-linear B-spline displacement retryably infeasible,
|
||||
# while the empty map still permits the independently revalidated coarse-path fallback.
|
||||
return New-Request @(
|
||||
(New-CoarsePoint 0.5 0.5 0.0 $forward 0.0),
|
||||
(New-CoarsePoint 1.0 0.5 0.5 $forward 0.0),
|
||||
(New-CoarsePoint 1.0 1.0 1.0 $forward 0.0),
|
||||
(New-CoarsePoint 1.5 1.0 1.5 $forward 0.0)) $Configuration
|
||||
}
|
||||
|
||||
function Invoke-Smooth($Request, [Threading.CancellationToken]$CancellationToken = [Threading.CancellationToken]::None) {
|
||||
return $smoothMethod.Invoke($service, @($Request, $CancellationToken))
|
||||
}
|
||||
|
||||
function Assert-NoGeometry($Result, [string]$Message) {
|
||||
Assert-Equal 0 $Result.Path.Count "$Message A non-published result must not expose a path."
|
||||
Assert-Equal 0 $Result.Segments.Count "$Message A non-published result must not expose segments."
|
||||
}
|
||||
|
||||
function Assert-InvalidInputBeforeRetry($Configuration, [string]$CaseName) {
|
||||
$result = Invoke-Smooth (New-StraightRequest $Configuration)
|
||||
Assert-Equal 'InvalidInput' $result.Status.ToString() "$CaseName must be rejected as invalid input."
|
||||
Assert-NoGeometry $result $CaseName
|
||||
Assert-Equal 0 $result.Diagnostics.RetryCount "$CaseName must be rejected before any smoothing retry."
|
||||
Assert-Near 0.0 $result.Diagnostics.AcceptedStrength 0.0 "$CaseName must not accept a smoothing strength."
|
||||
}
|
||||
|
||||
$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
|
||||
$facade = $root + 'Facade.'
|
||||
$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
|
||||
$mapping = 'MultiWheelC.TrajectoryPlanning.Mapping.'
|
||||
|
||||
$serviceType = Get-RequiredType ($facade + 'PathSmoothingService')
|
||||
$requestType = Get-RequiredType ($root + 'PathSmoothingRequest')
|
||||
$resultType = Get-RequiredType ($root + 'PathSmoothingResult')
|
||||
$configurationType = Get-RequiredType ($root + 'PathSmoothingConfiguration')
|
||||
$methodType = Get-RequiredType ($root + 'SmoothingMethod')
|
||||
$coarsePointType = Get-RequiredType ($coarsePath + 'CoarsePathPoint')
|
||||
$coarseSegmentType = Get-RequiredType ($coarsePath + 'PathSegment')
|
||||
$directionType = Get-RequiredType ($coarsePath + 'TravelDirection')
|
||||
$coarsePointSourceType = Get-RequiredType ($coarsePath + 'CoarsePathPointSource')
|
||||
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
|
||||
$boundsType = Get-RequiredType ($mapping + 'MapBoundsMm')
|
||||
$obstacleType = Get-RequiredType ($mapping + 'IMapObstacle')
|
||||
$rectangleType = Get-RequiredType ($mapping + 'AxisAlignedRectangleObstacle')
|
||||
$obstacleSourceType = Get-RequiredType ($mapping + 'IMapObstacleSource')
|
||||
$manualSourceType = Get-RequiredType ($mapping + 'ManualObstacleSource')
|
||||
$mapRequestType = Get-RequiredType ($mapping + 'PlanningMapRequest')
|
||||
$mapFactoryType = Get-RequiredType ($mapping + 'PlanningMapFactory')
|
||||
|
||||
Assert-True $serviceType.IsPublic 'PathSmoothingService must be public.'
|
||||
$service = [Activator]::CreateInstance($serviceType)
|
||||
$smoothMethod = $serviceType.GetMethod('Smooth', [Type[]]@($requestType, [Threading.CancellationToken]))
|
||||
Assert-True ($null -ne $smoothMethod) 'PathSmoothingService must expose Smooth(PathSmoothingRequest, CancellationToken).'
|
||||
Assert-Equal $resultType $smoothMethod.ReturnType 'PathSmoothingService Smooth must return PathSmoothingResult.'
|
||||
|
||||
$forward = [Enum]::Parse($directionType, 'Forward')
|
||||
$cubicBSpline = [Enum]::Parse($methodType, 'CubicBSpline')
|
||||
$localCubicBezier = [Enum]::Parse($methodType, 'LocalCubicBezier')
|
||||
$piecewiseQuintic = [Enum]::Parse($methodType, 'PiecewiseQuintic')
|
||||
$localG2Quintic = [Enum]::Parse($methodType, 'LocalG2Quintic')
|
||||
$coarseAnchor = [Enum]::Parse($coarsePointSourceType, 'Start')
|
||||
|
||||
# The public method registry must retain every stable enum-to-algorithm mapping.
|
||||
foreach ($method in @($cubicBSpline, $localCubicBezier, $piecewiseQuintic)) {
|
||||
$result = Invoke-Smooth (New-StraightRequest (New-Configuration $method))
|
||||
Assert-Equal 'Success' $result.Status.ToString() "A valid straight path must succeed for $method."
|
||||
Assert-Equal $method.ToString() $result.Method.ToString() "The result must retain the selected $method method."
|
||||
Assert-True ($result.Path.Count -gt 0) "A successful $method result must publish geometry."
|
||||
Assert-True $result.Diagnostics.Metrics.IsFeasible "A successful $method result must publish feasible diagnostics."
|
||||
}
|
||||
|
||||
# Local G2 is a dedicated service pipeline, but must remain available through the same public entrypoint.
|
||||
$localG2Result = Invoke-Smooth (New-StraightRequest (New-Configuration $localG2Quintic))
|
||||
Assert-Equal 'NotNeeded' $localG2Result.Status.ToString() 'A straight Local G2 request must publish its verified baseline as NotNeeded.'
|
||||
Assert-Equal 'LocalG2Quintic' $localG2Result.Method.ToString() 'Local G2 must retain the selected method.'
|
||||
Assert-True ($localG2Result.Path.Count -gt 0) 'NotNeeded Local G2 must publish verified geometry.'
|
||||
|
||||
# Every configuration scalar is checked before the options snapshot or retry runner starts.
|
||||
$invalidConfigurationCases = @(
|
||||
[PSCustomObject]@{ Name = 'NaN output spacing'; Mutate = { param($c) $c.OutputSpacingMeters = [double]::NaN } },
|
||||
[PSCustomObject]@{ Name = 'zero output spacing'; Mutate = { param($c) $c.OutputSpacingMeters = [double]0.0 } },
|
||||
[PSCustomObject]@{ Name = 'infinite collision step'; Mutate = { param($c) $c.MaximumCollisionCheckStepMeters = [double]::PositiveInfinity } },
|
||||
[PSCustomObject]@{ Name = 'zero collision step'; Mutate = { param($c) $c.MaximumCollisionCheckStepMeters = [double]0.0 } },
|
||||
[PSCustomObject]@{ Name = 'NaN clearance reserve'; Mutate = { param($c) $c.MinimumClearanceReserveMeters = [double]::NaN } },
|
||||
[PSCustomObject]@{ Name = 'negative clearance reserve'; Mutate = { param($c) $c.MinimumClearanceReserveMeters = [double]-0.01 } },
|
||||
[PSCustomObject]@{ Name = 'NaN smoothing strength'; Mutate = { param($c) $c.SmoothingStrength = [double]::NaN } },
|
||||
[PSCustomObject]@{ Name = 'zero smoothing strength'; Mutate = { param($c) $c.SmoothingStrength = [double]0.0 } },
|
||||
[PSCustomObject]@{ Name = 'NaN B-spline scale'; Mutate = { param($c) $c.CubicBSpline.EndpointTangentScale = [double]::NaN } },
|
||||
[PSCustomObject]@{ Name = 'zero B-spline scale'; Mutate = { param($c) $c.CubicBSpline.EndpointTangentScale = [double]0.0 } },
|
||||
[PSCustomObject]@{ Name = 'zero Bezier threshold'; Mutate = { param($c) $c.LocalCubicBezier.CornerHeadingThresholdRadians = [double]0.0 } },
|
||||
[PSCustomObject]@{ Name = 'over-pi Bezier threshold'; Mutate = { param($c) $c.LocalCubicBezier.CornerHeadingThresholdRadians = [Math]::PI + 0.01 } },
|
||||
[PSCustomObject]@{ Name = 'NaN Bezier window'; Mutate = { param($c) $c.LocalCubicBezier.MaximumWindowLengthMeters = [double]::NaN } },
|
||||
[PSCustomObject]@{ Name = 'zero Bezier window'; Mutate = { param($c) $c.LocalCubicBezier.MaximumWindowLengthMeters = [double]0.0 } },
|
||||
[PSCustomObject]@{ Name = 'infinite Bezier handle scale'; Mutate = { param($c) $c.LocalCubicBezier.HandleLengthRatio = [double]::PositiveInfinity } },
|
||||
[PSCustomObject]@{ Name = 'zero Bezier handle scale'; Mutate = { param($c) $c.LocalCubicBezier.HandleLengthRatio = [double]0.0 } },
|
||||
[PSCustomObject]@{ Name = 'NaN quintic knot spacing'; Mutate = { param($c) $c.PiecewiseQuintic.KnotSpacingMeters = [double]::NaN } },
|
||||
[PSCustomObject]@{ Name = 'zero quintic knot spacing'; Mutate = { param($c) $c.PiecewiseQuintic.KnotSpacingMeters = [double]0.0 } },
|
||||
[PSCustomObject]@{ Name = 'infinite minimum quintic knot spacing'; Mutate = { param($c) $c.PiecewiseQuintic.MinimumKnotSpacingMeters = [double]::PositiveInfinity } },
|
||||
[PSCustomObject]@{ Name = 'zero minimum quintic knot spacing'; Mutate = { param($c) $c.PiecewiseQuintic.MinimumKnotSpacingMeters = [double]0.0 } },
|
||||
[PSCustomObject]@{ Name = 'quintic knot spacing below minimum'; Mutate = { param($c) $c.PiecewiseQuintic.KnotSpacingMeters = [double]0.05; $c.PiecewiseQuintic.MinimumKnotSpacingMeters = [double]0.10 } }
|
||||
)
|
||||
foreach ($case in $invalidConfigurationCases) {
|
||||
$configuration = New-Configuration
|
||||
& $case.Mutate $configuration
|
||||
Assert-InvalidInputBeforeRetry $configuration $case.Name
|
||||
}
|
||||
|
||||
$unknownMethodConfiguration = New-Configuration ([Enum]::ToObject($methodType, 99))
|
||||
Assert-InvalidInputBeforeRetry $unknownMethodConfiguration 'unknown smoothing method'
|
||||
|
||||
$invalidCoarseConfiguration = New-Configuration
|
||||
$invalidCoarseRequest = New-Request @(
|
||||
(New-CoarsePoint ([double]::NaN) 0.5 0.0 $forward),
|
||||
(New-CoarsePoint 1.5 0.5 1.0 $forward)) $invalidCoarseConfiguration
|
||||
$invalidCoarseResult = Invoke-Smooth $invalidCoarseRequest
|
||||
Assert-Equal 'InvalidInput' $invalidCoarseResult.Status.ToString() 'A non-finite coarse path coordinate must be invalid input.'
|
||||
Assert-NoGeometry $invalidCoarseResult 'Invalid coarse path'
|
||||
Assert-Equal 0 $invalidCoarseResult.Diagnostics.RetryCount 'Invalid coarse input must be rejected before retries.'
|
||||
|
||||
# A finite, structurally valid coarse path may still be unsafe for the requested map and vehicle.
|
||||
# It must be rejected before method selection/retries and may never use fallback to publish the unsafe geometry.
|
||||
$unsafeCoarseResult = Invoke-Smooth (New-Request @(
|
||||
(New-CoarsePoint 0.5 0.5 0.0 $forward),
|
||||
(New-CoarsePoint 1.5 0.5 1.0 $forward)) (New-Configuration) (New-CollidingMap))
|
||||
Assert-Equal 'InvalidInput' $unsafeCoarseResult.Status.ToString() 'A colliding coarse path must be invalid before smoothing starts.'
|
||||
Assert-NoGeometry $unsafeCoarseResult 'Unsafe coarse path'
|
||||
Assert-Equal 0 $unsafeCoarseResult.Diagnostics.RetryCount 'Unsafe coarse geometry must be rejected before retry execution.'
|
||||
|
||||
$cancelledConfiguration = New-Configuration
|
||||
$cancellationSource = [Threading.CancellationTokenSource]::new()
|
||||
$cancellationSource.Cancel()
|
||||
try {
|
||||
$cancelledResult = Invoke-Smooth (New-StraightRequest $cancelledConfiguration) $cancellationSource.Token
|
||||
Assert-Equal 'Cancelled' $cancelledResult.Status.ToString() 'Pre-cancelled smoothing must return the explicit cancellation result.'
|
||||
Assert-NoGeometry $cancelledResult 'Cancelled smoothing'
|
||||
Assert-Equal 0 $cancelledResult.Diagnostics.RetryCount 'Cancellation before execution must not start retries.'
|
||||
}
|
||||
finally {
|
||||
$cancellationSource.Dispose()
|
||||
}
|
||||
|
||||
$withoutFallbackConfiguration = New-Configuration
|
||||
$withoutFallbackConfiguration.AllowFallbackToCoarsePath = $false
|
||||
$withoutFallbackResult = Invoke-Smooth (New-InfeasibleRequest $withoutFallbackConfiguration)
|
||||
Assert-Equal 'Infeasible' $withoutFallbackResult.Status.ToString() 'A retryably infeasible candidate without fallback must remain infeasible.'
|
||||
Assert-NoGeometry $withoutFallbackResult 'Infeasible smoothing without fallback'
|
||||
Assert-Equal 3 $withoutFallbackResult.Diagnostics.RetryCount 'Infeasible smoothing must exhaust the four configured strengths.'
|
||||
Assert-Near 0.0 $withoutFallbackResult.Diagnostics.AcceptedStrength 0.0 'Infeasible smoothing must not accept a strength.'
|
||||
|
||||
$withFallbackConfiguration = New-Configuration
|
||||
$withFallbackConfiguration.AllowFallbackToCoarsePath = $true
|
||||
$withFallbackResult = Invoke-Smooth (New-InfeasibleRequest $withFallbackConfiguration)
|
||||
Assert-Equal 'FallbackToCoarsePath' $withFallbackResult.Status.ToString() 'A verified coarse path must return explicit fallback status.'
|
||||
Assert-Equal 'CubicBSpline' $withFallbackResult.Method.ToString() 'Fallback must retain the originally selected method.'
|
||||
Assert-True ($withFallbackResult.Path.Count -gt 0) 'A verified fallback must publish the revalidated coarse geometry.'
|
||||
Assert-True $withFallbackResult.Diagnostics.Metrics.IsFeasible 'Fallback must publish feasible shared-geometry diagnostics.'
|
||||
foreach ($point in $withFallbackResult.Path) {
|
||||
Assert-Equal 'CoarsePathFallback' $point.Source.ToString() 'Every fallback point must be explicitly labeled as coarse-path fallback.'
|
||||
}
|
||||
Assert-Equal $withoutFallbackResult.Diagnostics.RetryCount $withFallbackResult.Diagnostics.RetryCount 'Fallback must preserve retry diagnostics from the failed method.'
|
||||
Assert-Near $withoutFallbackResult.Diagnostics.AcceptedStrength $withFallbackResult.Diagnostics.AcceptedStrength 0.0 'Fallback must preserve the failed method accepted-strength diagnostic.'
|
||||
Assert-Equal $withoutFallbackResult.Diagnostics.TerminationReason $withFallbackResult.Diagnostics.TerminationReason 'Fallback must preserve the failed method termination reason.'
|
||||
|
||||
Write-Output 'Path smoothing service checks passed.'
|
||||
@@ -0,0 +1,188 @@
|
||||
param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
|
||||
|
||||
$ErrorActionPreference = 'Stop'
|
||||
$newtonsoftPath = Join-Path $env:USERPROFILE '.nuget\packages\newtonsoft.json\13.0.4\lib\netstandard2.0\Newtonsoft.Json.dll'
|
||||
if (Test-Path -LiteralPath $newtonsoftPath) { [Reflection.Assembly]::LoadFrom($newtonsoftPath) | Out-Null }
|
||||
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
|
||||
|
||||
function Assert-Equal($Expected, $Actual, [string]$Message) {
|
||||
if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" }
|
||||
}
|
||||
|
||||
function Assert-True($Actual, [string]$Message) {
|
||||
if (-not $Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-Near([double]$Expected, [double]$Actual, [string]$Message) {
|
||||
if ([Math]::Abs($Expected - $Actual) -gt 0.000000001) {
|
||||
throw "$Message Expected=$Expected Actual=$Actual"
|
||||
}
|
||||
}
|
||||
|
||||
function Get-RequiredType([string]$Name) {
|
||||
return $assembly.GetType($Name, $true)
|
||||
}
|
||||
|
||||
function Text-FromCodePoints([int[]]$CodePoints) {
|
||||
return (($CodePoints | ForEach-Object { [Char]::ConvertFromUtf32($_) }) -join '')
|
||||
}
|
||||
|
||||
$visualization = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.Visualization.'
|
||||
$styleType = Get-RequiredType ($visualization + 'IeeeFigureStyle')
|
||||
|
||||
foreach ($expectation in @(
|
||||
@{ Name = 'FigureWidthPoints'; Value = 515.52 },
|
||||
@{ Name = 'FigureHeightPoints'; Value = 374.4 },
|
||||
@{ Name = 'RawColor'; Value = '#4D4D4D' },
|
||||
@{ Name = 'BSplineColor'; Value = '#0072B2' },
|
||||
@{ Name = 'BezierColor'; Value = '#D55E00' },
|
||||
@{ Name = 'QuinticColor'; Value = '#009E73' },
|
||||
@{ Name = 'LimitColor'; Value = '#CC79A7' }
|
||||
)) {
|
||||
$field = $styleType.GetField($expectation.Name)
|
||||
if ($null -eq $field) { throw "IeeeFigureStyle must expose $($expectation.Name)." }
|
||||
if ($expectation.Value -is [double]) {
|
||||
Assert-Near $expectation.Value $field.GetValue($null) "IeeeFigureStyle $($expectation.Name) must match the fixed report style."
|
||||
}
|
||||
else {
|
||||
Assert-Equal $expectation.Value $field.GetValue($null) "IeeeFigureStyle $($expectation.Name) must match the fixed report style."
|
||||
}
|
||||
}
|
||||
|
||||
$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
|
||||
$comparison = $root + 'Comparison.'
|
||||
$facade = $root + 'Facade.'
|
||||
$coarse = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
|
||||
$mapping = 'MultiWheelC.TrajectoryPlanning.Mapping.'
|
||||
$test = $root + 'Test.'
|
||||
|
||||
$modelType = Get-RequiredType ($visualization + 'SmoothingFigureModel')
|
||||
$builderType = Get-RequiredType ($visualization + 'SmoothingFigureModelBuilder')
|
||||
$figureDefinitionType = Get-RequiredType ($visualization + 'SmoothingFigureDefinition')
|
||||
$figureSetBuilderType = Get-RequiredType ($visualization + 'SmoothingFigureSetBuilder')
|
||||
$rendererType = Get-RequiredType ($visualization + 'SmoothingSvgRenderer')
|
||||
$csvWriterType = Get-RequiredType ($visualization + 'SmoothingCsvWriter')
|
||||
$comparisonResultType = Get-RequiredType ($comparison + 'PathSmoothingComparisonResult')
|
||||
$comparisonRequestType = Get-RequiredType ($comparison + 'PathSmoothingComparisonRequest')
|
||||
$comparisonServiceType = Get-RequiredType ($facade + 'PathSmoothingComparisonService')
|
||||
$scenarioFactoryType = Get-RequiredType ($test + 'SmoothingScenarioFactory')
|
||||
$gridMapType = Get-RequiredType ($mapping + 'PlanningGridMap')
|
||||
$poseType = Get-RequiredType ($coarse + 'Pose2D')
|
||||
|
||||
$buildMethod = $builderType.GetMethod('Build', [Type[]]@($comparisonResultType, $gridMapType, $poseType, $poseType, [string], [string]))
|
||||
if ($null -eq $buildMethod) { throw 'SmoothingFigureModelBuilder must expose Build(comparison, map, start, goal, scenarioId, scenarioLabel).' }
|
||||
Assert-Equal $modelType $buildMethod.ReturnType 'SmoothingFigureModelBuilder.Build must return the immutable figure model.'
|
||||
$figureSetBuildMethod = $figureSetBuilderType.GetMethod('Build', [Type[]]@($modelType))
|
||||
if ($null -eq $figureSetBuildMethod) { throw 'SmoothingFigureSetBuilder must expose Build(SmoothingFigureModel).' }
|
||||
$renderMethod = $rendererType.GetMethod('Render', [Type[]]@($figureDefinitionType))
|
||||
if ($null -eq $renderMethod) { throw 'SmoothingSvgRenderer must expose Render(SmoothingFigureDefinition).' }
|
||||
$writeMethod = $csvWriterType.GetMethod('Write', [Type[]]@($modelType))
|
||||
if ($null -eq $writeMethod) { throw 'SmoothingCsvWriter must expose Write(SmoothingFigureModel).' }
|
||||
|
||||
$fixturePath = Join-Path $PSScriptRoot '..\ParkrobTrajplanner\PathSmoothing\Test\Fixtures\path-smoothing-fixtures.json'
|
||||
$fixtureRequests = $scenarioFactoryType.GetMethod('CreateFixtureRequests', [Type[]]@([string])).Invoke($null, @((Resolve-Path $fixturePath).Path))
|
||||
$comparisonRequest = $fixtureRequests[0]
|
||||
Assert-Equal $comparisonRequestType $comparisonRequest.GetType() 'The figure test must consume a real immutable comparison request.'
|
||||
$comparisonResult = [Activator]::CreateInstance($comparisonServiceType).Compare($comparisonRequest, [Threading.CancellationToken]::None)
|
||||
$coarsePath = $comparisonRequest.SmoothingRequest.CoarsePath
|
||||
$start = [Activator]::CreateInstance($poseType, @($coarsePath[0].X, $coarsePath[0].Y, $coarsePath[0].Heading))
|
||||
$last = $coarsePath[$coarsePath.Count - 1]
|
||||
$goal = [Activator]::CreateInstance($poseType, @($last.X, $last.Y, $last.Heading))
|
||||
$builder = [Activator]::CreateInstance($builderType)
|
||||
$rawLabel = Text-FromCodePoints @(0x539F, 0x59CB, 0x7C97, 0x8DEF, 0x5F84)
|
||||
$bSplineLabel = (Text-FromCodePoints @(0x4E09, 0x6B21)) + ' B ' + (Text-FromCodePoints @(0x6837, 0x6761))
|
||||
$bezierLabel = (Text-FromCodePoints @(0x5C40, 0x90E8, 0x4E09, 0x6B21)) + ' B' + [Char]::ConvertFromUtf32(0x00E9) + 'zier'
|
||||
$quinticLabel = Text-FromCodePoints @(0x5206, 0x6BB5, 0x4E94, 0x6B21)
|
||||
$scenarioId = (Text-FromCodePoints @(0x573A, 0x666F)) + '-escape'
|
||||
$scenarioLabel = (Text-FromCodePoints @(0x8DEF, 0x5F84)) + ' <A&>'
|
||||
$model = $buildMethod.Invoke($builder, @($comparisonResult, $comparisonRequest.SmoothingRequest.Map, $start, $goal, $scenarioId, $scenarioLabel))
|
||||
|
||||
$figureSet = $figureSetBuildMethod.Invoke([Activator]::CreateInstance($figureSetBuilderType), @($model))
|
||||
$figures = @($figureSet.Figures)
|
||||
$expectedStems = @('01-coarse-path-overview', '02-all-paths-comparison', '03-cubic-bspline-overview', '04-local-cubic-bezier-overview', '05-piecewise-quintic-overview', '06-curvature-comparison')
|
||||
Assert-Equal ($expectedStems -join ',') (($figures | ForEach-Object { $_.FileStem }) -join ',') 'Figure set must publish the stable six-file order.'
|
||||
Assert-True (-not $figures[1].ShowsMapContext) 'All-path comparison must exclude obstacles and endpoint decorations.'
|
||||
Assert-True ($figures[2].ShowsMapContext -and $figures[2].Series[0].Opacity -lt 1) 'Individual smoother views must retain faded coarse-path context.'
|
||||
Assert-Near (($figures[0].WorldXMaxMeters - $figures[0].WorldXMinMeters) / ($figures[0].WorldYMaxMeters - $figures[0].WorldYMinMeters)) ($figures[0].PlotWidthPoints / $figures[0].PlotHeightPoints) 'Overhead figures must preserve equal X/Y scale.'
|
||||
|
||||
$renderer = [Activator]::CreateInstance($rendererType)
|
||||
$svgByStem = @{}
|
||||
foreach ($figure in $figures) { $svgByStem[$figure.FileStem] = $renderMethod.Invoke($renderer, @($figure)) }
|
||||
$svg = (($expectedStems | ForEach-Object { $svgByStem[$_] }) -join "`n")
|
||||
Assert-True ($svg -match '<svg[^>]+viewBox="0 0 515\.52 374\.4"') 'SVG must use the fixed physical view box in typographic points.'
|
||||
Assert-True $svg.Contains(($scenarioLabel -replace '&', '&' -replace '<', '<' -replace '>', '>')) 'SVG text must XML-escape scenario labels.'
|
||||
Assert-True $svg.Contains('font-family="SimSun"') 'SVG must emit explicit SimSun Chinese text runs.'
|
||||
Assert-True $svg.Contains('font-family="Times New Roman"') 'SVG must emit explicit Times New Roman Latin/math text runs.'
|
||||
foreach ($color in @('#4D4D4D', '#0072B2', '#D55E00', '#009E73', '#CC79A7')) { Assert-True $svg.Contains($color) "SVG must retain the fixed color $color." }
|
||||
Assert-True $svg.Contains('class="trajectory-point"') 'All visible trajectory samples must render as point markers.'
|
||||
Assert-True (-not $svg.Contains('stroke-dasharray')) 'Trajectory reports must not use dashed path styles.'
|
||||
Assert-True (-not $svg.Contains('<path')) 'Trajectory reports must not join samples with SVG path elements.'
|
||||
Assert-True $svgByStem['02-all-paths-comparison'].Contains('X (m)') 'Overhead comparison must label the X coordinate in metres.'
|
||||
Assert-True $svgByStem['06-curvature-comparison'].Contains('s (m)') 'Curvature comparison must label arc length in metres.'
|
||||
$curvatureUnit = [Char]::ConvertFromUtf32(0x03BA) + ' (m' + [Char]::ConvertFromUtf32(0x207B) + [Char]::ConvertFromUtf32(0x00B9) + ')'
|
||||
Assert-True $svgByStem['06-curvature-comparison'].Contains($curvatureUnit) 'Curvature comparison must label curvature units.'
|
||||
|
||||
$priorCulture = [Globalization.CultureInfo]::CurrentCulture
|
||||
try {
|
||||
[Globalization.CultureInfo]::CurrentCulture = [Globalization.CultureInfo]::GetCultureInfo('de-DE')
|
||||
[byte[]]$csvBytes = $writeMethod.Invoke([Activator]::CreateInstance($csvWriterType), @($model))
|
||||
}
|
||||
finally {
|
||||
[Globalization.CultureInfo]::CurrentCulture = $priorCulture
|
||||
}
|
||||
Assert-Equal 0xEF $csvBytes[0] 'CSV must begin with the UTF-8 BOM.'
|
||||
Assert-Equal 0xBB $csvBytes[1] 'CSV must begin with the UTF-8 BOM.'
|
||||
Assert-Equal 0xBF $csvBytes[2] 'CSV must begin with the UTF-8 BOM.'
|
||||
$csv = [Text.Encoding]::UTF8.GetString($csvBytes, 3, $csvBytes.Length - 3)
|
||||
$header = 'ScenarioId,Method,Status,PathLengthMeters,MaximumAbsoluteVehicleCurvaturePerMeter,RootMeanSquareVehicleCurvaturePerMeter,TotalAbsoluteCurvatureVariationPerMeter,CurvatureVariationEnergy,MinimumBodyClearanceMeters,MedianElapsedMilliseconds,TimingSampleCount,IsDeterministic,RetryCount,AcceptedStrength'
|
||||
Assert-True $csv.StartsWith($header + "`r`n") 'CSV must preserve the specified stable header order.'
|
||||
Assert-True $csv.Contains($scenarioId) 'CSV must preserve UTF-8 Chinese scenario content.'
|
||||
Assert-True ($csv -match ('(?m)^' + [Regex]::Escape($scenarioId) + ',[^,]+,[^,]+,\d+\.\d+')) 'CSV numeric values must use invariant decimal points.'
|
||||
|
||||
$entryType = Get-RequiredType ($comparison + 'PathSmoothingComparisonEntry')
|
||||
$methodType = Get-RequiredType ($root + 'SmoothingMethod')
|
||||
$statusType = Get-RequiredType ($root + 'PathSmoothingStatus')
|
||||
$metricsType = Get-RequiredType ($root + 'PathQualityMetrics')
|
||||
$pointType = Get-RequiredType ($root + 'SmoothedPathPoint')
|
||||
$segmentType = Get-RequiredType ($root + 'SmoothedPathSegment')
|
||||
$binding = [Reflection.BindingFlags]::Static -bor [Reflection.BindingFlags]::NonPublic
|
||||
$candidateFactory = $entryType.GetMethods($binding) | Where-Object { $_.Name -eq 'CreateCandidate' } | Select-Object -First 1
|
||||
if ($null -eq $candidateFactory) { throw 'Comparison entry must retain the internal candidate factory used by report diagnostics.' }
|
||||
$resultCtor = $comparisonResultType.GetConstructors([Reflection.BindingFlags]::Instance -bor [Reflection.BindingFlags]::NonPublic) |
|
||||
Where-Object { $_.GetParameters().Count -eq 5 } | Select-Object -First 1
|
||||
if ($null -eq $resultCtor) { throw 'Comparison result must retain its immutable internal constructor.' }
|
||||
$infeasible = [Enum]::Parse($statusType, 'Infeasible')
|
||||
$failed = [Enum]::Parse($statusType, 'Failed')
|
||||
$bSpline = [Enum]::Parse($methodType, 'CubicBSpline')
|
||||
$bezier = [Enum]::Parse($methodType, 'LocalCubicBezier')
|
||||
$quintic = [Enum]::Parse($methodType, 'PiecewiseQuintic')
|
||||
$raw = $comparisonResult.RawPathBaseline
|
||||
$timing = $comparisonResult.Entries[0].Timing
|
||||
$infeasibleEntry = $candidateFactory.Invoke($null, [object[]]@($bSpline, $infeasible, [Activator]::CreateInstance($metricsType), $timing, 'synthetic-infeasible', 'rejected geometry', 1, [double]0.5, $raw.Path, $raw.Segments))
|
||||
$emptyPoints = [Array]::CreateInstance($pointType, 0)
|
||||
$emptySegments = [Array]::CreateInstance($segmentType, 0)
|
||||
$failedEntry = $candidateFactory.Invoke($null, [object[]]@($bezier, $failed, [Activator]::CreateInstance($metricsType), $timing, 'synthetic-failure', 'numeric failure', 0, [double]0.0, $emptyPoints, $emptySegments))
|
||||
$quinticEntry = $comparisonResult.Entries | Where-Object { $_.Method.ToString() -eq $quintic.ToString() } | Select-Object -First 1
|
||||
$syntheticEntries = [Array]::CreateInstance($entryType, 3)
|
||||
$syntheticEntries.SetValue($infeasibleEntry, 0)
|
||||
$syntheticEntries.SetValue($failedEntry, 1)
|
||||
$syntheticEntries.SetValue($quinticEntry, 2)
|
||||
$syntheticResult = $resultCtor.Invoke([object[]]@($raw, $syntheticEntries, $null, $false, 'synthetic report diagnostics'))
|
||||
$syntheticModel = $buildMethod.Invoke($builder, @($syntheticResult, $comparisonRequest.SmoothingRequest.Map, $start, $goal, 'diagnostics', 'diagnostics'))
|
||||
$infeasibleSeries = $syntheticModel.Series | Where-Object { $_.Key -eq 'bspline' } | Select-Object -First 1
|
||||
$failedSeries = $syntheticModel.Series | Where-Object { $_.Key -eq 'bezier' } | Select-Object -First 1
|
||||
Assert-True $infeasibleSeries.IsCurveVisible 'A complete rejected candidate must remain visible in the report model.'
|
||||
Assert-True ($infeasibleSeries.ViolationMarkers.Count -gt 0) 'A rejected candidate must expose at least one violation cross marker.'
|
||||
Assert-True (-not $failedSeries.IsCurveVisible) 'A numerical failure with no geometry must not produce a fake curve.'
|
||||
$syntheticFigureSet = $figureSetBuildMethod.Invoke([Activator]::CreateInstance($figureSetBuilderType), @($syntheticModel))
|
||||
$syntheticSvg = $renderMethod.Invoke($renderer, @($syntheticFigureSet.Figures[2]))
|
||||
Assert-True $syntheticSvg.Contains('data-series="bspline"') 'Rejected candidate geometry must remain visible in the SVG.'
|
||||
Assert-True $syntheticSvg.Contains('violation-cross') 'Rejected candidate SVG geometry must include violation crosses.'
|
||||
Assert-True $syntheticSvg.Contains('Infeasible') 'Legend must retain the rejected candidate status.'
|
||||
Assert-True (-not $syntheticSvg.Contains('data-series="bezier"')) 'Numerical failure must not emit fake trajectory samples.'
|
||||
$syntheticCsvBytes = $writeMethod.Invoke([Activator]::CreateInstance($csvWriterType), @($syntheticModel))
|
||||
$syntheticCsv = [Text.Encoding]::UTF8.GetString($syntheticCsvBytes, 3, $syntheticCsvBytes.Length - 3)
|
||||
Assert-True $syntheticCsv.Contains('CubicBSpline,Infeasible') 'CSV must retain the rejected candidate status.'
|
||||
Assert-True $syntheticCsv.Contains('LocalCubicBezier,Failed') 'CSV must retain a numerical failure row.'
|
||||
|
||||
Write-Output 'Path smoothing SVG/CSV checks passed.'
|
||||
@@ -0,0 +1,175 @@
|
||||
param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
|
||||
|
||||
$ErrorActionPreference = 'Stop'
|
||||
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
|
||||
|
||||
function Assert-True($Actual, [string]$Message) {
|
||||
if (-not $Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-False($Actual, [string]$Message) {
|
||||
if ($Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-Near([double]$Expected, [double]$Actual, [double]$Tolerance, [string]$Message) {
|
||||
if ([Math]::Abs($Expected - $Actual) -gt $Tolerance) {
|
||||
throw "$Message Expected=$Expected Actual=$Actual Tolerance=$Tolerance"
|
||||
}
|
||||
}
|
||||
|
||||
function Get-RequiredType([string]$Name) {
|
||||
return $assembly.GetType($Name, $true)
|
||||
}
|
||||
|
||||
function New-Map([bool]$WithObstacle) {
|
||||
$bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]4000, [single]0, [single]4000))
|
||||
$request = [Activator]::CreateInstance($mapRequestType)
|
||||
$request.Bounds = $bounds
|
||||
$request.ResolutionMm = [single]50
|
||||
if ($WithObstacle) {
|
||||
$obstacle = [Activator]::CreateInstance($rectangleType, @([single]1900, [single]2100, [single]1800, [single]2200))
|
||||
$obstacles = [Array]::CreateInstance($obstacleType, 1)
|
||||
$obstacles.SetValue($obstacle, 0)
|
||||
$source = [Activator]::CreateInstance($manualSourceType, @('validator-obstacle', [long]1, $true, $obstacles))
|
||||
$sources = [Array]::CreateInstance($obstacleSourceType, 1)
|
||||
$sources.SetValue($source, 0)
|
||||
$request.ObstacleSources = $sources
|
||||
} else {
|
||||
$request.ObstacleSources = [Array]::CreateInstance($obstacleSourceType, 0)
|
||||
$request.AllowExplicitEmptyMap = $true
|
||||
}
|
||||
$result = [Activator]::CreateInstance($mapFactoryType).Create($request)
|
||||
Assert-True $result.Succeeded 'Validation test map must be built.'
|
||||
Assert-True $result.Map.PlanningReady 'Validation test map must be ready.'
|
||||
return $result.Map
|
||||
}
|
||||
|
||||
function New-SmoothedPoint([double]$X, [double]$Y, [double]$ArcLength, $Direction,
|
||||
[double]$VehicleCurvature = 0.0, [bool]$IsGearSwitch = $false, [double]$Clearance = 999.0,
|
||||
[double]$VehicleCurvatureDerivative = 0.0) {
|
||||
return [Activator]::CreateInstance($smoothedPointType, @(
|
||||
$X, $Y, [double]0.0, [double]0.0, $ArcLength, $Direction,
|
||||
$VehicleCurvature, $VehicleCurvature, $VehicleCurvatureDerivative, $Clearance, $IsGearSwitch, $anchor))
|
||||
}
|
||||
|
||||
function New-PreparedPoint([double]$X, [double]$Y, [double]$ArcLength, [bool]$IsGearSwitch = $false) {
|
||||
return [Activator]::CreateInstance($smoothingPointType, @(
|
||||
$X, $Y, $ArcLength, [double]0.0, [double]0.0, [double]999.0, $IsGearSwitch, $anchor))
|
||||
}
|
||||
|
||||
function New-OneSegmentCase([double]$X0, [double]$Y0, [double]$X1, [double]$Y1, [double]$VehicleCurvature = 0.0,
|
||||
[double]$StartArcLength = 0.0, [double]$VehicleCurvatureDerivative = 0.0) {
|
||||
$candidatePath = [Array]::CreateInstance($smoothedPointType, 2)
|
||||
$candidatePath.SetValue((New-SmoothedPoint $X0 $Y0 $StartArcLength $forward 0.0 $false 999.0 $VehicleCurvatureDerivative), 0)
|
||||
$candidatePath.SetValue((New-SmoothedPoint $X1 $Y1 ($StartArcLength + 1.0) $forward $VehicleCurvature $false 999.0 $VehicleCurvatureDerivative), 1)
|
||||
$candidateSegments = [Array]::CreateInstance($smoothedSegmentType, 1)
|
||||
$candidateSegments.SetValue([Activator]::CreateInstance($smoothedSegmentType, @(0, $forward, 0, 1, $false, $false)), 0)
|
||||
$preparedPoints = [Array]::CreateInstance($smoothingPointType, 2)
|
||||
$preparedPoints.SetValue((New-PreparedPoint $X0 $Y0 0.0), 0)
|
||||
$preparedPoints.SetValue((New-PreparedPoint $X1 $Y1 1.0), 1)
|
||||
$preparedSegments = [Array]::CreateInstance($preparedSegmentType, 1)
|
||||
$preparedSegments.SetValue([Activator]::CreateInstance($preparedSegmentType, @(0, $forward, $preparedPoints, $false, $false)), 0)
|
||||
return [PSCustomObject]@{
|
||||
CandidatePath = $candidatePath
|
||||
CandidateSegments = $candidateSegments
|
||||
Original = [Activator]::CreateInstance($preparedPathType, [object[]]@(,$preparedSegments))
|
||||
}
|
||||
}
|
||||
|
||||
function Invoke-Validation($Case, $Map) {
|
||||
$arguments = [object[]]@($Case.CandidatePath, $Case.CandidateSegments, $Case.Original, $Map, $vehicle,
|
||||
[double]0.05, $null, [double]0.0, $null)
|
||||
$accepted = $validateMethod.Invoke($validator, $arguments)
|
||||
return [PSCustomObject]@{ Accepted = $accepted; Path = $arguments[6]; MinimumClearance = $arguments[7]; Reason = $arguments[8] }
|
||||
}
|
||||
|
||||
function New-MovedGearSwitchCase {
|
||||
$candidatePath = [Array]::CreateInstance($smoothedPointType, 4)
|
||||
$candidatePath.SetValue((New-SmoothedPoint 0.5 0.5 0.0 $forward), 0)
|
||||
$candidatePath.SetValue((New-SmoothedPoint 1.5 0.5 1.0 $forward), 1)
|
||||
$candidatePath.SetValue((New-SmoothedPoint 1.6 0.5 1.0 $reverse 0.0 $true), 2)
|
||||
$candidatePath.SetValue((New-SmoothedPoint 0.5 0.5 2.0 $reverse), 3)
|
||||
$candidateSegments = [Array]::CreateInstance($smoothedSegmentType, 2)
|
||||
$candidateSegments.SetValue([Activator]::CreateInstance($smoothedSegmentType, @(0, $forward, 0, 1, $false, $true)), 0)
|
||||
$candidateSegments.SetValue([Activator]::CreateInstance($smoothedSegmentType, @(1, $reverse, 2, 3, $true, $false)), 1)
|
||||
$preparedFirst = [Array]::CreateInstance($smoothingPointType, 2)
|
||||
$preparedFirst.SetValue((New-PreparedPoint 0.5 0.5 0.0), 0)
|
||||
$preparedFirst.SetValue((New-PreparedPoint 1.5 0.5 1.0), 1)
|
||||
$preparedSecond = [Array]::CreateInstance($smoothingPointType, 2)
|
||||
$preparedSecond.SetValue((New-PreparedPoint 1.5 0.5 0.0 $true), 0)
|
||||
$preparedSecond.SetValue((New-PreparedPoint 0.5 0.5 1.0), 1)
|
||||
$preparedSegments = [Array]::CreateInstance($preparedSegmentType, 2)
|
||||
$preparedSegments.SetValue([Activator]::CreateInstance($preparedSegmentType, @(0, $forward, $preparedFirst, $false, $true)), 0)
|
||||
$preparedSegments.SetValue([Activator]::CreateInstance($preparedSegmentType, @(1, $reverse, $preparedSecond, $true, $false)), 1)
|
||||
return [PSCustomObject]@{
|
||||
CandidatePath = $candidatePath
|
||||
CandidateSegments = $candidateSegments
|
||||
Original = [Activator]::CreateInstance($preparedPathType, [object[]]@(,$preparedSegments))
|
||||
}
|
||||
}
|
||||
|
||||
$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
|
||||
$processing = $root + 'Processing.'
|
||||
$validation = $root + 'Validation.'
|
||||
$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
|
||||
$mapping = 'MultiWheelC.TrajectoryPlanning.Mapping.'
|
||||
|
||||
$validatorType = Get-RequiredType ($validation + 'SmoothedPathValidator')
|
||||
$smoothedPointType = Get-RequiredType ($root + 'SmoothedPathPoint')
|
||||
$smoothedSegmentType = Get-RequiredType ($root + 'SmoothedPathSegment')
|
||||
$smoothingPointType = Get-RequiredType ($processing + 'SmoothingPoint2D')
|
||||
$preparedSegmentType = Get-RequiredType ($processing + 'PreparedDirectionSegment')
|
||||
$preparedPathType = Get-RequiredType ($processing + 'PreparedPath')
|
||||
$directionType = Get-RequiredType ($coarsePath + 'TravelDirection')
|
||||
$sourceType = Get-RequiredType ($root + 'SmoothedPathPointSource')
|
||||
$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
|
||||
$boundsType = Get-RequiredType ($mapping + 'MapBoundsMm')
|
||||
$obstacleType = Get-RequiredType ($mapping + 'IMapObstacle')
|
||||
$rectangleType = Get-RequiredType ($mapping + 'AxisAlignedRectangleObstacle')
|
||||
$obstacleSourceType = Get-RequiredType ($mapping + 'IMapObstacleSource')
|
||||
$manualSourceType = Get-RequiredType ($mapping + 'ManualObstacleSource')
|
||||
$mapRequestType = Get-RequiredType ($mapping + 'PlanningMapRequest')
|
||||
$mapFactoryType = Get-RequiredType ($mapping + 'PlanningMapFactory')
|
||||
|
||||
$validator = [Activator]::CreateInstance($validatorType)
|
||||
$validateMethod = $validatorType.GetMethod('TryValidate')
|
||||
Assert-True ($null -ne $validateMethod) 'SmoothedPathValidator must expose TryValidate.'
|
||||
Assert-True ($validateMethod.GetParameters().Length -eq 9) 'TryValidate must accept path, segments, originals, map, vehicle, step, path, clearance, and reason.'
|
||||
$forward = [Enum]::Parse($directionType, 'Forward')
|
||||
$reverse = [Enum]::Parse($directionType, 'Reverse')
|
||||
$anchor = [Enum]::Parse($sourceType, 'Anchor')
|
||||
$vehicle = [Activator]::CreateInstance($vehicleType)
|
||||
$vehicle.LengthMeters = 0.20
|
||||
$vehicle.WidthMeters = 0.20
|
||||
$vehicle.SafetyMarginMeters = 0.0
|
||||
$vehicle.MaximumCurvaturePerMeter = 1.0
|
||||
$vehicle.MinimumTurningRadiusMeters = 1.0
|
||||
|
||||
$emptyMap = New-Map $false
|
||||
$obstacleMap = New-Map $true
|
||||
$valid = Invoke-Validation (New-OneSegmentCase 0.5 0.5 1.5 0.5) $obstacleMap
|
||||
Assert-True $valid.Accepted ('A valid straight candidate must pass. Reason=' + $valid.Reason)
|
||||
Assert-True ($null -ne $valid.Path) 'A valid candidate must return clearance-recomputed points.'
|
||||
Assert-True ($valid.Path[0].BodyClearance -lt 999.0) 'Validated output must replace an overclaimed candidate clearance.'
|
||||
Assert-True ($valid.MinimumClearance -ge 0.0) 'A valid candidate must report non-negative conservative clearance.'
|
||||
$derivativePreserved = Invoke-Validation (New-OneSegmentCase 0.5 0.5 1.5 0.5 0.0 0.0 0.25) $obstacleMap
|
||||
Assert-True $derivativePreserved.Accepted ('A valid derivative-bearing candidate must pass. Reason=' + $derivativePreserved.Reason)
|
||||
Assert-Near 0.25 $derivativePreserved.Path[0].VehicleCurvatureDerivative 0.000000001 `
|
||||
'Clearance recomputation must preserve the first point curvature derivative.'
|
||||
Assert-Near 0.25 $derivativePreserved.Path[1].VehicleCurvatureDerivative 0.000000001 `
|
||||
'Clearance recomputation must preserve the final point curvature derivative.'
|
||||
|
||||
$pointCollision = Invoke-Validation (New-OneSegmentCase 2.0 2.0 2.5 2.0) $obstacleMap
|
||||
Assert-False $pointCollision.Accepted 'A smoothing candidate that touches an obstacle must be rejected.'
|
||||
$sweptCollision = Invoke-Validation (New-OneSegmentCase 1.5 2.0 2.5 2.0) $obstacleMap
|
||||
Assert-False $sweptCollision.Accepted 'A smoothing candidate whose sweep cuts through an obstacle must be rejected.'
|
||||
$outsideBounds = Invoke-Validation (New-OneSegmentCase 0.05 0.5 0.5 0.5) $emptyMap
|
||||
Assert-False $outsideBounds.Accepted 'A smoothing candidate outside map bounds must be rejected.'
|
||||
$overCurvature = Invoke-Validation (New-OneSegmentCase 0.5 0.5 1.5 0.5 2.0) $emptyMap
|
||||
Assert-False $overCurvature.Accepted 'A smoothing candidate above vehicle maximum curvature must be rejected.'
|
||||
$nonzeroStartArc = Invoke-Validation (New-OneSegmentCase 0.5 0.5 1.5 0.5 0.0 1.0) $emptyMap
|
||||
Assert-False $nonzeroStartArc.Accepted 'A smoothing candidate must start at global arc length zero.'
|
||||
$movedSwitch = Invoke-Validation (New-MovedGearSwitchCase) $emptyMap
|
||||
Assert-False $movedSwitch.Accepted 'A smoothing candidate with a moved gear-switch pose must be rejected.'
|
||||
|
||||
Write-Output 'Path smoothing validation checks passed.'
|
||||
@@ -0,0 +1,47 @@
|
||||
param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
|
||||
$ErrorActionPreference = 'Stop'
|
||||
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
|
||||
function Assert-True($Actual, [string]$Message) { if (-not $Actual) { throw $Message } }
|
||||
function Assert-Equal($Expected, $Actual, [string]$Message) { if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" } }
|
||||
$boundsType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.MapBoundsMm', $true)
|
||||
$gridType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.EnvironmentGridMap', $true)
|
||||
$circleType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.CircleObstacle', $true)
|
||||
$rectangleType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.AxisAlignedRectangleObstacle', $true)
|
||||
$rasterizer = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.MapObstacleRasterizer', $true)
|
||||
$adapter = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Mapping.PlanningMapAdapter', $true)
|
||||
$bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]105, [single]0, [single]105))
|
||||
$grid = [Activator]::CreateInstance($gridType, @($bounds, [single]20))
|
||||
Assert-Equal $true ($grid.IsWorldInBounds([single]104.9, [single]104.9)) 'Last partial cell must be inside.'
|
||||
Assert-Equal $false ($grid.IsWorldInBounds([single]105, [single]50)) 'XMax must be exclusive.'
|
||||
Assert-Equal $true ($grid.IsOccupiedWorld([single]-0.1, [single]20)) 'Outside world must be occupied.'
|
||||
$circle = [Activator]::CreateInstance($circleType, @([single]40, [single]40, [single]0))
|
||||
$rasterizer.GetMethod('Rasterize').Invoke($null, @($grid, $circle)) | Out-Null
|
||||
Assert-True ($grid.OccupiedCount -ge 4) 'Circle touching a grid intersection must conservatively occupy adjacent cells.'
|
||||
$rectangle = [Activator]::CreateInstance($rectangleType, @([single]80, [single]100, [single]80, [single]100))
|
||||
$rasterizer.GetMethod('Rasterize').Invoke($null, @($grid, $rectangle)) | Out-Null
|
||||
Assert-True ($grid.IsOccupiedWorld([single]90, [single]90)) 'Rectangle must occupy its intersecting cells.'
|
||||
$outside = [Activator]::CreateInstance($circleType, @([single]500, [single]500, [single]10))
|
||||
$before = $grid.OccupiedCount
|
||||
$rasterizer.GetMethod('Rasterize').Invoke($null, @($grid, $outside)) | Out-Null
|
||||
Assert-Equal $before $grid.OccupiedCount 'Outside obstacle must not mark cells.'
|
||||
$planning = $adapter.GetMethod('Create').Invoke($null, @($grid))
|
||||
Assert-True ($planning.IsOccupiedWorld(0.04, 0.04)) 'Planning map must use metre world coordinates.'
|
||||
Assert-Equal 0.0 ($planning.GetConservativeObstacleDistanceMeters(0.04, 0.04)) 'Occupied cell clearance must be zero.'
|
||||
Assert-Equal 0.0 ($planning.GetConservativeObstacleDistanceMeters(-1.0, 0.0)) 'Outside map clearance must be zero.'
|
||||
$singleBounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]3000, [single]0, [single]3000))
|
||||
$singleGrid = [Activator]::CreateInstance($gridType, @($singleBounds, [single]50))
|
||||
$singleCircle = [Activator]::CreateInstance($circleType, @([single]1525, [single]1525, [single]0))
|
||||
$rasterizer.GetMethod('Rasterize').Invoke($null, @($singleGrid, $singleCircle)) | Out-Null
|
||||
Assert-Equal 1 $singleGrid.OccupiedCount 'Zero radius circle at a cell center must occupy exactly one cell.'
|
||||
$singlePlanning = $adapter.GetMethod('Create').Invoke($null, @($singleGrid))
|
||||
$singleDistance = $singlePlanning.GetConservativeObstacleDistanceMeters(1.275, 1.275)
|
||||
$singleNearestCellCenterDistance = [Math]::Sqrt(0.25 * 0.25 + 0.25 * 0.25)
|
||||
Assert-True ((-not [double]::IsNaN($singleDistance)) -and (-not [double]::IsInfinity($singleDistance))) 'Single occupied cell clearance must be finite.'
|
||||
Assert-True ($singleDistance -le $singleNearestCellCenterDistance) 'Single occupied cell clearance must not exceed nearest occupied cell geometry distance.'
|
||||
$emptyGrid = [Activator]::CreateInstance($gridType, @($bounds, [single]20))
|
||||
$emptyPlanning = $adapter.GetMethod('Create').Invoke($null, @($emptyGrid))
|
||||
Assert-True ([double]::IsPositiveInfinity($emptyPlanning.GetConservativeObstacleDistanceMeters(0.01, 0.01))) 'Empty map clearance must be positive infinity.'
|
||||
$invalidResolutionRejected = $false
|
||||
try { [Activator]::CreateInstance($gridType, @($bounds, [single]10)) | Out-Null } catch { $invalidResolutionRejected = $true }
|
||||
Assert-True $invalidResolutionRejected 'Resolution below 20 mm must fail.'
|
||||
Write-Output 'Planning map adapter checks passed.'
|
||||
@@ -0,0 +1,58 @@
|
||||
param([string]$MapRoot = (Join-Path $PSScriptRoot '..\ParkrobTrajplanner\Map'))
|
||||
|
||||
$ErrorActionPreference = 'Stop'
|
||||
|
||||
function Assert-Contains([string]$Content, [string]$Expected, [string]$Message) {
|
||||
if (-not $Content.Contains($Expected)) { throw "$Message Missing=$Expected" }
|
||||
}
|
||||
|
||||
function Assert-DocumentationCount([string]$RelativePath, [int]$MinimumCount) {
|
||||
$path = Join-Path $MapRoot $RelativePath
|
||||
$content = Get-Content -LiteralPath $path -Raw
|
||||
$count = [regex]::Matches($content, '/// <summary>').Count
|
||||
if ($count -lt $MinimumCount) { throw "Insufficient public API documentation in $RelativePath ExpectedAtLeast=$MinimumCount Actual=$count" }
|
||||
}
|
||||
|
||||
$readmePath = Join-Path $MapRoot 'README.md'
|
||||
if (-not (Test-Path -LiteralPath $readmePath)) { throw "Map README is required: $readmePath" }
|
||||
$readme = Get-Content -LiteralPath $readmePath -Raw
|
||||
Assert-Contains $readme 'PlanningMapFactory' 'README must identify the public creation facade.'
|
||||
Assert-Contains $readme 'IMapObstacleSource' 'README must explain unified obstacle sources.'
|
||||
Assert-Contains $readme 'PlanningGridMap' 'README must explain the planning snapshot output.'
|
||||
Assert-Contains $readme 'SourceVersion' 'README must explain cache invalidation versions.'
|
||||
Assert-Contains $readme 'PlanningMapBuildStatus' 'README must explain explicit map build termination states.'
|
||||
Assert-Contains $readme 'PNG' 'README must explain debug image output.'
|
||||
Assert-Contains $readme 'TrapMap' 'README must describe the legacy-map boundary.'
|
||||
|
||||
Assert-DocumentationCount 'PlanningMapFactory.cs' 2
|
||||
Assert-DocumentationCount 'PlanningMapRequest.cs' 5
|
||||
Assert-DocumentationCount 'PlanningMapBuildResult.cs' 9
|
||||
Assert-DocumentationCount 'Core\MapBuildRequest.cs' 4
|
||||
Assert-DocumentationCount 'Core\EnvironmentMapBuildResult.cs' 7
|
||||
Assert-DocumentationCount 'Core\MapBoundsMm.cs' 12
|
||||
Assert-DocumentationCount 'Core\EnvironmentGridMap.cs' 13
|
||||
Assert-DocumentationCount 'Core\EnvironmentMapBuilder.cs' 2
|
||||
Assert-DocumentationCount 'Obstacles\IMapObstacle.cs' 2
|
||||
Assert-DocumentationCount 'Obstacles\CircleObstacle.cs' 6
|
||||
Assert-DocumentationCount 'Obstacles\AxisAlignedRectangleObstacle.cs' 7
|
||||
Assert-DocumentationCount 'Obstacles\MapObstacleRasterizer.cs' 2
|
||||
Assert-DocumentationCount 'Sources\IMapObstacleSource.cs' 5
|
||||
Assert-DocumentationCount 'Sources\ManualObstacleSource.cs' 6
|
||||
Assert-DocumentationCount 'Sources\TwoLegProjectionInput.cs' 13
|
||||
Assert-DocumentationCount 'Sources\TwoLegObstacleSource.cs' 6
|
||||
Assert-DocumentationCount 'Sources\TwoLegObstacleProjector.cs' 2
|
||||
Assert-DocumentationCount 'Sources\ObstacleProjectionResult.cs' 8
|
||||
Assert-DocumentationCount 'Sources\ObstacleSourceStatus.cs' 5
|
||||
Assert-DocumentationCount 'Planning\PlanningGridMap.cs' 15
|
||||
Assert-DocumentationCount 'Planning\PlanningMapAdapter.cs' 2
|
||||
Assert-DocumentationCount 'Planning\ObstacleDistanceField.cs' 2
|
||||
Assert-DocumentationCount 'Planning\EuclideanDistanceTransform.cs' 2
|
||||
Assert-DocumentationCount 'Planning\PlanningMapCache.cs' 5
|
||||
Assert-DocumentationCount 'Test\MovementTest.MapTest.cs' 3
|
||||
Assert-DocumentationCount 'Test\Visualization\PlanningMapImageExportRequest.cs' 3
|
||||
Assert-DocumentationCount 'Test\Visualization\PlanningMapImageExportResult.cs' 8
|
||||
Assert-DocumentationCount 'Test\Visualization\PlanningMapImageExporter.cs' 6
|
||||
Assert-DocumentationCount 'Test\Visualization\PlanningMapImageRenderer.cs' 2
|
||||
Assert-DocumentationCount 'Test\Visualization\ValidatedPngWriter.cs' 2
|
||||
|
||||
Write-Output 'Planning map documentation checks passed.'
|
||||
@@ -0,0 +1,88 @@
|
||||
param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
|
||||
$ErrorActionPreference = 'Stop'
|
||||
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
|
||||
function Assert-True($Actual, [string]$Message) { if (-not $Actual) { throw $Message } }
|
||||
function Assert-Equal($Expected, $Actual, [string]$Message) { if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" } }
|
||||
function Assert-False($Actual, [string]$Message) { if ($Actual) { throw $Message } }
|
||||
function Assert-Null($Actual, [string]$Message) { if ($null -ne $Actual) { throw $Message } }
|
||||
function Find-NonPublicInstanceMethod($Type, [string]$Name, [Type[]]$ParameterTypes) {
|
||||
foreach ($candidate in $Type.GetMethods([Reflection.BindingFlags]'Instance,NonPublic')) {
|
||||
if ($candidate.Name -ne $Name) { continue }
|
||||
$parameters = $candidate.GetParameters()
|
||||
if ($parameters.Length -ne $ParameterTypes.Length) { continue }
|
||||
$matches = $true
|
||||
for ($index = 0; $index -lt $parameters.Length; $index++) {
|
||||
if ($parameters[$index].ParameterType -ne $ParameterTypes[$index]) { $matches = $false; break }
|
||||
}
|
||||
if ($matches) { return $candidate }
|
||||
}
|
||||
return $null
|
||||
}
|
||||
$ns = 'MultiWheelC.TrajectoryPlanning.Mapping.'
|
||||
$boundsType = $assembly.GetType($ns + 'MapBoundsMm', $true)
|
||||
$obstacleType = $assembly.GetType($ns + 'IMapObstacle', $true)
|
||||
$sourceType = $assembly.GetType($ns + 'IMapObstacleSource', $true)
|
||||
$circleType = $assembly.GetType($ns + 'CircleObstacle', $true)
|
||||
$manualType = $assembly.GetType($ns + 'ManualObstacleSource', $true)
|
||||
$twoLegInputType = $assembly.GetType($ns + 'TwoLegProjectionInput', $true)
|
||||
$twoLegType = $assembly.GetType($ns + 'TwoLegObstacleSource', $true)
|
||||
$requestType = $assembly.GetType($ns + 'PlanningMapRequest', $true)
|
||||
$factoryType = $assembly.GetType($ns + 'PlanningMapFactory', $true)
|
||||
$mapResultType = $assembly.GetType($ns + 'PlanningMapBuildResult', $true)
|
||||
$mapBuildStatusType = $assembly.GetType($ns + 'PlanningMapBuildStatus', $true)
|
||||
$operationBudgetType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Utils.PlanningOperationBudget', $false)
|
||||
$operationStopReasonType = $assembly.GetType('MultiWheelC.TrajectoryPlanning.Utils.PlanningOperationStopReason', $false)
|
||||
Assert-True ($operationBudgetType -ne $null) 'PlanningOperationBudget must exist for shared planning cancellation and timeout handling.'
|
||||
Assert-True ($operationStopReasonType -ne $null) 'PlanningOperationStopReason must exist for shared planning cancellation and timeout handling.'
|
||||
Assert-True $operationStopReasonType.IsEnum 'PlanningOperationStopReason must be an enum.'
|
||||
foreach ($expectedStopReason in @('None', 'Cancelled', 'TimedOut')) {
|
||||
Assert-True ($operationStopReasonType.GetEnumNames() -contains $expectedStopReason) "PlanningOperationStopReason must contain $expectedStopReason."
|
||||
}
|
||||
Assert-True ($mapResultType.GetProperty('Status') -ne $null) 'PlanningMapBuildResult must expose an explicit build status.'
|
||||
foreach ($expectedMapStatus in @('Success', 'Failed', 'Cancelled', 'TimedOut')) {
|
||||
Assert-True ($mapBuildStatusType.GetEnumNames() -contains $expectedMapStatus) "PlanningMapBuildStatus must contain $expectedMapStatus."
|
||||
}
|
||||
$bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]2000, [single]0, [single]2000))
|
||||
$obstacles = [Array]::CreateInstance($obstacleType, 1)
|
||||
$obstacles.SetValue([Activator]::CreateInstance($circleType, @([single]500, [single]500, [single]100)), 0)
|
||||
$manual = [Activator]::CreateInstance($manualType, @('manual', [long]1, $true, $obstacles))
|
||||
$twoLegInput = [Activator]::CreateInstance($twoLegInputType, @($true, [single]1000, [single]1000, [double]([Math]::PI / 2), [single]100, [single]0, [single]-100, [single]0, [single]40, 'test snapshot'))
|
||||
$twoLeg = [Activator]::CreateInstance($twoLegType, @('two-leg', [long]1, $false, $twoLegInput))
|
||||
$sources = [Array]::CreateInstance($sourceType, 2); $sources.SetValue($twoLeg, 0); $sources.SetValue($manual, 1)
|
||||
function New-Request($sourceArray, [bool]$allowEmpty) { $request = [Activator]::CreateInstance($requestType); $request.Bounds = $bounds; $request.ResolutionMm = [single]50; $request.ObstacleSources = $sourceArray; $request.AllowExplicitEmptyMap = $allowEmpty; return $request }
|
||||
$factory = [Activator]::CreateInstance($factoryType)
|
||||
$budgetConstructor = $operationBudgetType.GetConstructor([Reflection.BindingFlags]'Instance,NonPublic', $null,
|
||||
@([Threading.CancellationToken], [TimeSpan]), $null)
|
||||
Assert-True ($budgetConstructor -ne $null) 'PlanningOperationBudget must expose its internal cancellation and timeout constructor.'
|
||||
$createWithBudget = Find-NonPublicInstanceMethod $factoryType 'Create' @($requestType, $operationBudgetType)
|
||||
Assert-True ($createWithBudget -ne $null) 'PlanningMapFactory must expose an internal budget-aware Create overload.'
|
||||
$cancelledSource = New-Object Threading.CancellationTokenSource
|
||||
$cancelledSource.Cancel()
|
||||
$cancelledBudget = $budgetConstructor.Invoke(@($cancelledSource.Token, [TimeSpan]::FromSeconds(1)))
|
||||
$cancelled = $createWithBudget.Invoke($factory, @((New-Request $sources $false), $cancelledBudget))
|
||||
Assert-Equal 'Cancelled' $cancelled.Status.ToString() 'Cancelled map construction must retain the cancellation status.'
|
||||
Assert-False $cancelled.Succeeded 'Cancelled map construction must not succeed.'
|
||||
Assert-Null $cancelled.Map 'Cancelled map construction must not publish a map.'
|
||||
Assert-Equal 'None' $cancelled.CacheHit.ToString() 'Cancelled map construction must not publish a cache hit.'
|
||||
$first = $factory.Create((New-Request $sources $false))
|
||||
Assert-True $first.Succeeded 'Mixed source request must build.'
|
||||
Assert-True $first.Map.PlanningReady 'Applied source geometry must make the map ready.'
|
||||
Assert-True ($first.Map.IsOccupiedWorld(1.0, 1.1)) 'TwoLeg must project detection-time local coordinates into world metres.'
|
||||
$second = $factory.Create((New-Request $sources $false))
|
||||
Assert-Equal 'Input' $second.CacheHit.ToString() 'Same snapshot must hit complete input cache.'
|
||||
Assert-True ([object]::ReferenceEquals($first.Map, $second.Map)) 'Same snapshot must return the exact immutable map object.'
|
||||
$manualVersionTwo = [Activator]::CreateInstance($manualType, @('manual', [long]2, $true, $obstacles))
|
||||
$sourcesVersionTwo = [Array]::CreateInstance($sourceType, 2); $sourcesVersionTwo.SetValue($manualVersionTwo, 0); $sourcesVersionTwo.SetValue($twoLeg, 1)
|
||||
$third = $factory.Create((New-Request $sourcesVersionTwo $false))
|
||||
Assert-Equal 'Occupancy' $third.CacheHit.ToString() 'Version change with equal occupancy must reuse occupancy buffers.'
|
||||
Assert-True ($third.Map.SnapshotId -gt $first.Map.SnapshotId) 'Occupancy reuse must still issue a fresh snapshot id.'
|
||||
$emptySources = [Array]::CreateInstance($sourceType, 0)
|
||||
$emptyBlocked = $factory.Create((New-Request $emptySources $false))
|
||||
Assert-True (-not $emptyBlocked.Map.PlanningReady) 'Implicit empty map must be blocked.'
|
||||
$emptyAllowed = $factory.Create((New-Request $emptySources $true))
|
||||
Assert-True $emptyAllowed.Map.PlanningReady 'Explicit empty map must be accepted.'
|
||||
$invalidManual = [Activator]::CreateInstance($manualType, @('invalid', [long]0, $true, $null))
|
||||
$invalidSources = [Array]::CreateInstance($sourceType, 1); $invalidSources.SetValue($invalidManual, 0)
|
||||
$invalid = $factory.Create((New-Request $invalidSources $false))
|
||||
Assert-True (-not $invalid.Succeeded) 'Required invalid source must reject the full build.'
|
||||
Write-Output 'Planning map factory checks passed.'
|
||||
@@ -0,0 +1,31 @@
|
||||
param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
|
||||
$ErrorActionPreference = 'Stop'
|
||||
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
|
||||
function Assert-True($Actual, [string]$Message) { if (-not $Actual) { throw $Message } }
|
||||
$ns = 'MultiWheelC.TrajectoryPlanning.Mapping.'
|
||||
$boundsType = $assembly.GetType($ns + 'MapBoundsMm', $true)
|
||||
$sourceType = $assembly.GetType($ns + 'IMapObstacleSource', $true)
|
||||
$requestType = $assembly.GetType($ns + 'PlanningMapRequest', $true)
|
||||
$factoryType = $assembly.GetType($ns + 'PlanningMapFactory', $true)
|
||||
$imageRequestType = $assembly.GetType($ns + 'PlanningMapImageExportRequest', $true)
|
||||
$exporterType = $assembly.GetType($ns + 'PlanningMapImageExporter', $true)
|
||||
$mapRequest = [Activator]::CreateInstance($requestType)
|
||||
$mapRequest.Bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]1000, [single]0, [single]1000))
|
||||
$mapRequest.ResolutionMm = [single]50
|
||||
$mapRequest.ObstacleSources = [Array]::CreateInstance($sourceType, 0)
|
||||
$mapRequest.AllowExplicitEmptyMap = $true
|
||||
$map = [Activator]::CreateInstance($factoryType).Create($mapRequest).Map
|
||||
$imageRequest = [Activator]::CreateInstance($imageRequestType)
|
||||
$imageRequest.Map = $map
|
||||
$imageRequest.OutputRootDirectory = (Join-Path $PSScriptRoot '..\obj\planning_map_image_test')
|
||||
$export = $exporterType.GetMethod('ExportIfEnabled').Invoke($null, @($true, $imageRequest))
|
||||
Assert-True $export.Saved ('Image export failed: ' + $export.Message)
|
||||
Assert-True (Test-Path -LiteralPath $export.FilePath -PathType Leaf) 'Image output is missing.'
|
||||
$png = [IO.File]::ReadAllBytes($export.FilePath)
|
||||
Assert-True ($png.Length -gt 45) 'PNG is too small.'
|
||||
Assert-True ($png[0] -eq 137 -and $png[1] -eq 80 -and $png[2] -eq 78 -and $png[3] -eq 71) 'PNG signature is invalid.'
|
||||
Assert-True ([Text.Encoding]::ASCII.GetString($png, 12, 4) -eq 'IHDR') 'PNG must begin with IHDR.'
|
||||
Assert-True ([Text.Encoding]::ASCII.GetString($png, $png.Length - 8, 4) -eq 'IEND') 'PNG must end with IEND.'
|
||||
$source = Get-Content -LiteralPath (Join-Path $PSScriptRoot '..\ParkrobTrajplanner\Map\Test\Visualization\PlanningMapImageExporter.cs') -Raw -Encoding UTF8
|
||||
Assert-True ($source -notmatch 'GridMapData|TrapMapVehiclePose|TwoLegDetect') 'New exporter must consume only PlanningGridMap.'
|
||||
Write-Output 'Planning map image checks passed.'
|
||||
@@ -0,0 +1,23 @@
|
||||
param([string]$SourcePath = (Join-Path $PSScriptRoot '..\ParkrobTrajplanner\Map\Test\MovementTest.MapTest.cs'))
|
||||
|
||||
$ErrorActionPreference = 'Stop'
|
||||
$source = Get-Content -LiteralPath $SourcePath -Raw
|
||||
|
||||
function Assert-Contains([string]$Expected, [string]$Message) {
|
||||
if (-not $source.Contains($Expected)) { throw "$Message Missing=$Expected" }
|
||||
}
|
||||
|
||||
Assert-Contains 'EnableTerminalDebugLog' 'MapTest must expose a terminal logging switch.'
|
||||
Assert-Contains 'SavePng' 'MapTest must expose a PNG export switch.'
|
||||
$begin = -join [char[]](0x89C4, 0x5212, 0x5730, 0x56FE, 0x521B, 0x5EFA, 0x5F00, 0x59CB)
|
||||
$sources = -join [char[]](0x969C, 0x788D, 0x7269, 0x6765)
|
||||
$projection = -join [char[]](0x6295, 0x5F71, 0x7ED3, 0x679C)
|
||||
$snapshot = -join [char[]](0x5730, 0x56FE, 0x5FEB, 0x7167)
|
||||
$end = -join [char[]](0x89C4, 0x5212, 0x5730, 0x56FE, 0x521B, 0x5EFA, 0x5B8C, 0x6210)
|
||||
Assert-Contains 'FormatSourceStatus' 'MapTest must translate projection status for terminal output.'
|
||||
Assert-Contains 'FormatCacheHit' 'MapTest must translate cache hits for terminal output.'
|
||||
Assert-Contains 'FormatImageResult' 'MapTest must translate image export results for terminal output.'
|
||||
if ($source.Contains('PLANNING_MAP_CREATE_BEGIN')) { throw 'MapTest must not expose English terminal log markers.' }
|
||||
Assert-Contains 'PlanningMapFactory' 'MapTest must continue using the public map facade.'
|
||||
|
||||
Write-Output 'Planning map MovementTest configuration checks passed.'
|
||||
Reference in New Issue
Block a user