test: add path smoothing scenarios and fixtures
This commit is contained in:
梁薄云
+19451
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,260 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Globalization;
|
||||
using System.IO;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Test;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using Newtonsoft.Json;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Test;
|
||||
|
||||
/// <summary>显式开发入口：从已成功的 Hybrid A* 粗路径结果生成八个稳定的快速比较夹具。</summary>
|
||||
public static class SmoothingFixtureGenerator
|
||||
{
|
||||
// 仅用于开发时固化真实规划输出；不会改变业务场景或运行时服务的预算。
|
||||
private static readonly TimeSpan FixtureGenerationSearchTimeout = TimeSpan.FromSeconds(120d);
|
||||
|
||||
/// <summary>生成夹具 JSON；目标已存在且未明确允许覆盖时拒绝写入。</summary>
|
||||
public static void Generate(string outputPath, bool overwrite)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(outputPath)) throw new ArgumentException("输出路径不能为空。", nameof(outputPath));
|
||||
if (File.Exists(outputPath) && !overwrite)
|
||||
throw new IOException("夹具目标已存在；必须显式允许覆盖。");
|
||||
|
||||
SmoothingFixtureDocument document = CreateDocument();
|
||||
Directory.CreateDirectory(Path.GetDirectoryName(Path.GetFullPath(outputPath)));
|
||||
var settings = new JsonSerializerSettings { Culture = CultureInfo.InvariantCulture, Formatting = Formatting.Indented };
|
||||
File.WriteAllText(outputPath, JsonConvert.SerializeObject(document, settings), new System.Text.UTF8Encoding(false));
|
||||
}
|
||||
|
||||
private static SmoothingFixtureDocument CreateDocument()
|
||||
{
|
||||
var document = new SmoothingFixtureDocument { SchemaVersion = 1 };
|
||||
var planner = new CoarsePathPlanningService();
|
||||
IReadOnlyList<FixturePlan> plans = CreatePlans();
|
||||
for (int index = 0; index < plans.Count; index++)
|
||||
{
|
||||
FixturePlan plan = plans[index];
|
||||
CoarsePathPlanningJob job = plan.CreateJob();
|
||||
job.Configuration.SearchTimeout = FixtureGenerationSearchTimeout;
|
||||
CoarsePathPlanningJobResult result = planner.Plan(job, CancellationToken.None);
|
||||
document.Scenarios.Add(CreateRecord(plan.Id, job, result));
|
||||
}
|
||||
|
||||
for (int index = 0; index < document.Scenarios.Count; index++)
|
||||
document.Scenarios[index].ConfigurationFingerprint = SmoothingScenarioFixtureLoader.ComputeFingerprint(document.Scenarios[index]);
|
||||
return document;
|
||||
}
|
||||
|
||||
private static IReadOnlyList<FixturePlan> CreatePlans()
|
||||
{
|
||||
return new List<FixturePlan>
|
||||
{
|
||||
new FixturePlan("straight", () => CoarsePathScenarioFactory.Create(CoarsePathTestScenario.ExplicitEmpty)),
|
||||
new FixturePlan("single-turn", () => CoarsePathScenarioFactory.CreateManualGoalDemo(1000d, 1000d, 0d, 3000d, 2000d, 45d)),
|
||||
new FixturePlan("s-bend", () => CoarsePathScenarioFactory.Create(CoarsePathTestScenario.ManualAndTwoLeg, 1000d, 1000d, 45d)),
|
||||
new FixturePlan("large-heading-change", () => CoarsePathScenarioFactory.CreateManualGoalDemo(1000d, 1000d, 0d, 3000d, 3000d, 90d)),
|
||||
new FixturePlan("rectangle-detour", () => CoarsePathScenarioFactory.Create(CoarsePathTestScenario.RectangleDetour)),
|
||||
new FixturePlan("multi-obstacle-detour", () => CoarsePathScenarioFactory.Create(CoarsePathTestScenario.ManualAndTwoLeg)),
|
||||
new FixturePlan("narrow-corridor", CreateNarrowCorridor),
|
||||
new FixturePlan("forward-reverse-switch", () => CoarsePathScenarioFactory.Create(CoarsePathTestScenario.ReverseGearSwitch)),
|
||||
};
|
||||
}
|
||||
|
||||
private static CoarsePathPlanningJob CreateNarrowCorridor()
|
||||
{
|
||||
return CreateFixedObstacleJob("narrow-corridor", new IMapObstacle[]
|
||||
{
|
||||
new AxisAlignedRectangleObstacle(1500f, 4500f, 500f, 1200f),
|
||||
new AxisAlignedRectangleObstacle(1500f, 4500f, 2800f, 3500f),
|
||||
});
|
||||
}
|
||||
|
||||
private static CoarsePathPlanningJob CreateFixedObstacleJob(string id, IReadOnlyList<IMapObstacle> obstacles)
|
||||
{
|
||||
var job = new CoarsePathPlanningJob
|
||||
{
|
||||
MapRequest = new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(0f, 6000f, 0f, 4000f),
|
||||
ResolutionMm = 50f,
|
||||
AllowExplicitEmptyMap = false,
|
||||
ObstacleSources = new IMapObstacleSource[]
|
||||
{
|
||||
new ManualObstacleSource("fixture-" + id, 1L, true, obstacles),
|
||||
},
|
||||
},
|
||||
Start = new Pose2D(1d, 2d, 0d),
|
||||
Goal = new Pose2D(5d, 2d, 0d),
|
||||
Vehicle = new VehicleParameters
|
||||
{
|
||||
LengthMeters = 0.80d,
|
||||
WidthMeters = 0.60d,
|
||||
SafetyMarginMeters = 0.05d,
|
||||
MaximumCurvaturePerMeter = 1d / 1.20d,
|
||||
},
|
||||
Configuration = new HybridAStarConfiguration(),
|
||||
GoalDirection = GoalDirectionConstraint.Forward,
|
||||
};
|
||||
job.Configuration.SearchTimeout = TimeSpan.FromSeconds(30d);
|
||||
return job;
|
||||
}
|
||||
|
||||
private static SmoothingFixtureRecord CreateRecord(string id, CoarsePathPlanningJob job, CoarsePathPlanningJobResult result)
|
||||
{
|
||||
if (job == null || result == null || result.MapResult == null || !result.MapResult.Succeeded ||
|
||||
result.MapResult.Map == null || result.PlanningResult == null || result.PlanningResult.Status != PlanningStatus.Success)
|
||||
{
|
||||
string status = result == null || result.PlanningResult == null ? "no-result" : result.PlanningResult.Status.ToString();
|
||||
string reason = result == null || result.PlanningResult == null || result.PlanningResult.Diagnostics == null
|
||||
? string.Empty
|
||||
: result.PlanningResult.Diagnostics.TerminationReason;
|
||||
throw new InvalidOperationException("夹具场景未产生成功的粗路径：" + id + "；状态=" + status + "；原因=" + reason);
|
||||
}
|
||||
|
||||
PlanningGridMap map = result.MapResult.Map;
|
||||
IReadOnlyList<CoarsePathPoint> smoothingPath = SmoothingScenarioFactory.CopyWithFiniteClearance(result.PlanningResult.Path, map);
|
||||
var record = new SmoothingFixtureRecord
|
||||
{
|
||||
Id = id,
|
||||
FixtureVersion = 1,
|
||||
Map = CreateMapRecord(map, result.MapResult.SourceResults),
|
||||
Vehicle = CreateVehicleRecord(job.Vehicle),
|
||||
PlanningConfiguration = CreatePlanningConfigurationRecord(job.Configuration),
|
||||
};
|
||||
|
||||
for (int index = 0; index < smoothingPath.Count; index++)
|
||||
record.Path.Add(CreatePathPointRecord(smoothingPath[index]));
|
||||
for (int index = 0; index < result.PlanningResult.Segments.Count; index++)
|
||||
record.Segments.Add(CreateSegmentRecord(result.PlanningResult.Segments[index]));
|
||||
return record;
|
||||
}
|
||||
|
||||
private static SmoothingFixtureMap CreateMapRecord(PlanningGridMap map, IReadOnlyList<ObstacleProjectionResult> sourceResults)
|
||||
{
|
||||
var record = new SmoothingFixtureMap
|
||||
{
|
||||
XMinMm = map.Bounds.XMin,
|
||||
XMaxMm = map.Bounds.XMax,
|
||||
YMinMm = map.Bounds.YMin,
|
||||
YMaxMm = map.Bounds.YMax,
|
||||
ResolutionMm = map.ResolutionMm,
|
||||
};
|
||||
for (int sourceIndex = 0; sourceIndex < sourceResults.Count; sourceIndex++)
|
||||
{
|
||||
IReadOnlyList<IMapObstacle> obstacles = sourceResults[sourceIndex].Obstacles;
|
||||
for (int obstacleIndex = 0; obstacleIndex < obstacles.Count; obstacleIndex++)
|
||||
record.Obstacles.Add(CreateObstacleRecord(obstacles[obstacleIndex]));
|
||||
}
|
||||
return record;
|
||||
}
|
||||
|
||||
private static SmoothingFixtureObstacle CreateObstacleRecord(IMapObstacle obstacle)
|
||||
{
|
||||
if (obstacle is CircleObstacle circle)
|
||||
{
|
||||
return new SmoothingFixtureObstacle
|
||||
{
|
||||
Kind = "circle",
|
||||
CenterXMm = circle.CenterX,
|
||||
CenterYMm = circle.CenterY,
|
||||
RadiusMm = circle.RadiusMm,
|
||||
};
|
||||
}
|
||||
if (obstacle is AxisAlignedRectangleObstacle rectangle)
|
||||
{
|
||||
return new SmoothingFixtureObstacle
|
||||
{
|
||||
Kind = "axis-aligned-rectangle",
|
||||
XMinMm = rectangle.XMin,
|
||||
XMaxMm = rectangle.XMax,
|
||||
YMinMm = rectangle.YMin,
|
||||
YMaxMm = rectangle.YMax,
|
||||
};
|
||||
}
|
||||
throw new InvalidOperationException("夹具仅支持圆形和轴对齐矩形障碍物。" + obstacle?.GetType().FullName);
|
||||
}
|
||||
|
||||
private static SmoothingFixtureVehicle CreateVehicleRecord(VehicleParameters vehicle)
|
||||
{
|
||||
if (!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvaturePerMeter))
|
||||
throw new InvalidOperationException("夹具车辆缺少有效最大曲率限制。");
|
||||
return new SmoothingFixtureVehicle
|
||||
{
|
||||
LengthMeters = vehicle.LengthMeters,
|
||||
WidthMeters = vehicle.WidthMeters,
|
||||
SafetyMarginMeters = vehicle.SafetyMarginMeters,
|
||||
MaximumCurvaturePerMeter = maximumCurvaturePerMeter,
|
||||
};
|
||||
}
|
||||
|
||||
private static SmoothingFixturePlanningConfiguration CreatePlanningConfigurationRecord(HybridAStarConfiguration configuration)
|
||||
{
|
||||
return new SmoothingFixturePlanningConfiguration
|
||||
{
|
||||
PrimitiveLengthMeters = configuration.PrimitiveLengthMeters,
|
||||
IntegrationStepMeters = configuration.IntegrationStepMeters,
|
||||
MaximumCollisionCheckStepMeters = configuration.MaximumCollisionCheckStepMeters,
|
||||
HeadingResolutionRadians = configuration.HeadingResolutionRadians,
|
||||
CurvatureLevelCount = configuration.CurvatureLevelCount,
|
||||
GoalPositionToleranceMeters = configuration.GoalPositionToleranceMeters,
|
||||
GoalHeadingToleranceRadians = configuration.GoalHeadingToleranceRadians,
|
||||
MaximumExpandedNodes = configuration.MaximumExpandedNodes,
|
||||
SearchTimeoutSeconds = configuration.SearchTimeout.TotalSeconds,
|
||||
HeuristicWeight = configuration.HeuristicWeight,
|
||||
ReverseCostMultiplier = configuration.ReverseCostMultiplier,
|
||||
GearSwitchPenaltyMeters = configuration.GearSwitchPenaltyMeters,
|
||||
CurvatureMagnitudeWeight = configuration.CurvatureMagnitudeWeight,
|
||||
CurvatureChangePenaltyMetersPerLevel = configuration.CurvatureChangePenaltyMetersPerLevel,
|
||||
ClearanceCostWeight = configuration.ClearanceCostWeight,
|
||||
ClearanceCostDistanceMeters = configuration.ClearanceCostDistanceMeters,
|
||||
AllowReverse = configuration.AllowReverse,
|
||||
};
|
||||
}
|
||||
|
||||
private static SmoothingFixturePathPoint CreatePathPointRecord(CoarsePathPoint point)
|
||||
{
|
||||
return new SmoothingFixturePathPoint
|
||||
{
|
||||
XMeters = point.X,
|
||||
YMeters = point.Y,
|
||||
HeadingRadians = point.Heading,
|
||||
UnwrappedHeadingRadians = point.UnwrappedHeading,
|
||||
ArcLengthMeters = point.ArcLength,
|
||||
Direction = point.Direction,
|
||||
VehicleCurvaturePerMeter = point.VehicleCurvature,
|
||||
BodyClearanceMeters = point.BodyClearance,
|
||||
IsGearSwitchPoint = point.IsGearSwitchPoint,
|
||||
Source = point.Source,
|
||||
};
|
||||
}
|
||||
|
||||
private static SmoothingFixtureSegment CreateSegmentRecord(PathSegment segment)
|
||||
{
|
||||
return new SmoothingFixtureSegment
|
||||
{
|
||||
SegmentIndex = segment.SegmentIndex,
|
||||
Direction = segment.Direction,
|
||||
StartIndex = segment.StartIndex,
|
||||
EndIndex = segment.EndIndex,
|
||||
StartsAtGearSwitch = segment.StartsAtGearSwitch,
|
||||
EndsAtGearSwitch = segment.EndsAtGearSwitch,
|
||||
};
|
||||
}
|
||||
|
||||
private sealed class FixturePlan
|
||||
{
|
||||
public FixturePlan(string id, Func<CoarsePathPlanningJob> createJob)
|
||||
{
|
||||
Id = id;
|
||||
CreateJob = createJob;
|
||||
}
|
||||
|
||||
public string Id { get; }
|
||||
public Func<CoarsePathPlanningJob> CreateJob { get; }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,69 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Test;
|
||||
|
||||
/// <summary>将快速夹具或现有粗路径业务结果转换为独立平滑比较请求。</summary>
|
||||
public static class SmoothingScenarioFactory
|
||||
{
|
||||
/// <summary>不运行 Hybrid A*，从已验证的 JSON 夹具创建八个快速比较请求。</summary>
|
||||
public static IReadOnlyList<PathSmoothingComparisonRequest> CreateFixtureRequests(string fixturePath)
|
||||
{
|
||||
IReadOnlyList<SmoothingScenarioFixture> fixtures = SmoothingScenarioFixtureLoader.LoadAndVerify(fixturePath);
|
||||
var requests = new List<PathSmoothingComparisonRequest>(fixtures.Count);
|
||||
for (int index = 0; index < fixtures.Count; index++)
|
||||
{
|
||||
SmoothingScenarioFixture fixture = fixtures[index];
|
||||
requests.Add(new PathSmoothingComparisonRequest(
|
||||
new PathSmoothingRequest(
|
||||
fixture.Path,
|
||||
fixture.Segments,
|
||||
SmoothingScenarioFixtureLoader.BuildMap(fixture),
|
||||
SmoothingScenarioFixtureLoader.BuildVehicle(fixture),
|
||||
new PathSmoothingConfiguration())));
|
||||
}
|
||||
return requests.AsReadOnly();
|
||||
}
|
||||
|
||||
/// <summary>把现有 <see cref="CoarsePathScenarioFactory"/> 已规划成功的结果转换为比较请求。</summary>
|
||||
public static PathSmoothingComparisonRequest CreateEndToEndRequest(
|
||||
CoarsePathPlanningJob job,
|
||||
CoarsePathPlanningJobResult result)
|
||||
{
|
||||
if (job == null || result == null || result.MapResult == null || result.MapResult.Map == null ||
|
||||
result.PlanningResult == null || result.PlanningResult.Status != PlanningStatus.Success)
|
||||
throw new ArgumentException("只能从包含成功地图和粗路径的业务结果创建平滑比较请求。", nameof(result));
|
||||
|
||||
return new PathSmoothingComparisonRequest(new PathSmoothingRequest(
|
||||
CopyWithFiniteClearance(result.PlanningResult.Path, result.MapResult.Map),
|
||||
result.PlanningResult.Segments,
|
||||
result.MapResult.Map,
|
||||
job.Vehicle,
|
||||
new PathSmoothingConfiguration()));
|
||||
}
|
||||
|
||||
internal static IReadOnlyList<CoarsePathPoint> CopyWithFiniteClearance(
|
||||
IReadOnlyList<CoarsePathPoint> source,
|
||||
MultiWheelC.TrajectoryPlanning.Mapping.PlanningGridMap map)
|
||||
{
|
||||
double widthMeters = (map.Bounds.XMax - map.Bounds.XMin) / 1000d;
|
||||
double heightMeters = (map.Bounds.YMax - map.Bounds.YMin) / 1000d;
|
||||
double finiteEmptyMapClearance = Math.Sqrt(widthMeters * widthMeters + heightMeters * heightMeters);
|
||||
var copy = new List<CoarsePathPoint>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
{
|
||||
CoarsePathPoint point = source[index];
|
||||
double clearance = double.IsPositiveInfinity(point.BodyClearance)
|
||||
? finiteEmptyMapClearance
|
||||
: point.BodyClearance;
|
||||
copy.Add(new CoarsePathPoint(
|
||||
point.X, point.Y, point.Heading, point.UnwrappedHeading, point.ArcLength, point.Direction,
|
||||
point.VehicleCurvature, clearance, point.IsGearSwitchPoint, point.Source));
|
||||
}
|
||||
return copy.AsReadOnly();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,156 @@
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using Newtonsoft.Json;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Test;
|
||||
|
||||
/// <summary>一个可复现的快速粗路径夹具及其地图、车辆和方向段快照。</summary>
|
||||
public sealed class SmoothingScenarioFixture
|
||||
{
|
||||
internal SmoothingScenarioFixture(SmoothingFixtureRecord record, bool fingerprintCurrent)
|
||||
{
|
||||
Record = record;
|
||||
Id = record.Id;
|
||||
FixtureVersion = record.FixtureVersion;
|
||||
ConfigurationFingerprint = record.ConfigurationFingerprint;
|
||||
IsConfigurationFingerprintCurrent = fingerprintCurrent;
|
||||
Path = ToPath(record.Path);
|
||||
Segments = ToSegments(record.Segments);
|
||||
}
|
||||
|
||||
/// <summary>稳定英文场景标识。</summary>
|
||||
public string Id { get; }
|
||||
/// <summary>夹具自身的正版本号。</summary>
|
||||
public int FixtureVersion { get; }
|
||||
/// <summary>由夹具输入生成的稳定 SHA-256 指纹。</summary>
|
||||
public string ConfigurationFingerprint { get; }
|
||||
/// <summary>存储指纹是否与当前夹具内容一致。</summary>
|
||||
public bool IsConfigurationFingerprintCurrent { get; }
|
||||
/// <summary>快速比较使用的不可变粗路径点。</summary>
|
||||
public IReadOnlyList<CoarsePathPoint> Path { get; }
|
||||
/// <summary>覆盖粗路径的不可变方向段。</summary>
|
||||
public IReadOnlyList<PathSegment> Segments { get; }
|
||||
|
||||
internal SmoothingFixtureRecord Record { get; }
|
||||
|
||||
private static IReadOnlyList<CoarsePathPoint> ToPath(IReadOnlyList<SmoothingFixturePathPoint> source)
|
||||
{
|
||||
var result = new List<CoarsePathPoint>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
{
|
||||
SmoothingFixturePathPoint point = source[index];
|
||||
result.Add(new CoarsePathPoint(
|
||||
point.XMeters, point.YMeters, point.HeadingRadians, point.UnwrappedHeadingRadians,
|
||||
point.ArcLengthMeters, point.Direction, point.VehicleCurvaturePerMeter,
|
||||
point.BodyClearanceMeters, point.IsGearSwitchPoint, point.Source));
|
||||
}
|
||||
return new ReadOnlyCollection<CoarsePathPoint>(result);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<PathSegment> ToSegments(IReadOnlyList<SmoothingFixtureSegment> source)
|
||||
{
|
||||
var result = new List<PathSegment>(source.Count);
|
||||
for (int index = 0; index < source.Count; index++)
|
||||
{
|
||||
SmoothingFixtureSegment segment = source[index];
|
||||
result.Add(new PathSegment(segment.SegmentIndex, segment.Direction, segment.StartIndex, segment.EndIndex,
|
||||
segment.StartsAtGearSwitch, segment.EndsAtGearSwitch));
|
||||
}
|
||||
return new ReadOnlyCollection<PathSegment>(result);
|
||||
}
|
||||
}
|
||||
|
||||
internal sealed class SmoothingFixtureDocument
|
||||
{
|
||||
[JsonProperty("schemaVersion")] public int SchemaVersion { get; set; }
|
||||
[JsonProperty("scenarios")] public List<SmoothingFixtureRecord> Scenarios { get; set; } = new List<SmoothingFixtureRecord>();
|
||||
}
|
||||
|
||||
internal sealed class SmoothingFixtureRecord
|
||||
{
|
||||
[JsonProperty("id")] public string Id { get; set; }
|
||||
[JsonProperty("fixtureVersion")] public int FixtureVersion { get; set; }
|
||||
[JsonProperty("configurationFingerprint")] public string ConfigurationFingerprint { get; set; }
|
||||
[JsonProperty("map")] public SmoothingFixtureMap Map { get; set; }
|
||||
[JsonProperty("vehicle")] public SmoothingFixtureVehicle Vehicle { get; set; }
|
||||
[JsonProperty("planningConfiguration")] public SmoothingFixturePlanningConfiguration PlanningConfiguration { get; set; }
|
||||
[JsonProperty("path")] public List<SmoothingFixturePathPoint> Path { get; set; } = new List<SmoothingFixturePathPoint>();
|
||||
[JsonProperty("segments")] public List<SmoothingFixtureSegment> Segments { get; set; } = new List<SmoothingFixtureSegment>();
|
||||
}
|
||||
|
||||
internal sealed class SmoothingFixtureMap
|
||||
{
|
||||
[JsonProperty("xMinMm")] public float XMinMm { get; set; }
|
||||
[JsonProperty("xMaxMm")] public float XMaxMm { get; set; }
|
||||
[JsonProperty("yMinMm")] public float YMinMm { get; set; }
|
||||
[JsonProperty("yMaxMm")] public float YMaxMm { get; set; }
|
||||
[JsonProperty("resolutionMm")] public float ResolutionMm { get; set; }
|
||||
[JsonProperty("obstacles")] public List<SmoothingFixtureObstacle> Obstacles { get; set; } = new List<SmoothingFixtureObstacle>();
|
||||
}
|
||||
|
||||
internal sealed class SmoothingFixtureObstacle
|
||||
{
|
||||
[JsonProperty("kind")] public string Kind { get; set; }
|
||||
[JsonProperty("xMinMm")] public float XMinMm { get; set; }
|
||||
[JsonProperty("xMaxMm")] public float XMaxMm { get; set; }
|
||||
[JsonProperty("yMinMm")] public float YMinMm { get; set; }
|
||||
[JsonProperty("yMaxMm")] public float YMaxMm { get; set; }
|
||||
[JsonProperty("centerXMm")] public float CenterXMm { get; set; }
|
||||
[JsonProperty("centerYMm")] public float CenterYMm { get; set; }
|
||||
[JsonProperty("radiusMm")] public float RadiusMm { get; set; }
|
||||
}
|
||||
|
||||
internal sealed class SmoothingFixtureVehicle
|
||||
{
|
||||
[JsonProperty("lengthMeters")] public double LengthMeters { get; set; }
|
||||
[JsonProperty("widthMeters")] public double WidthMeters { get; set; }
|
||||
[JsonProperty("safetyMarginMeters")] public double SafetyMarginMeters { get; set; }
|
||||
[JsonProperty("maximumCurvaturePerMeter")] public double MaximumCurvaturePerMeter { get; set; }
|
||||
}
|
||||
|
||||
/// <summary>产生夹具粗路径时使用的 Hybrid A* 配置快照；仅用于溯源和指纹验证，不参与快速夹具加载时的规划。</summary>
|
||||
internal sealed class SmoothingFixturePlanningConfiguration
|
||||
{
|
||||
[JsonProperty("primitiveLengthMeters")] public double PrimitiveLengthMeters { get; set; }
|
||||
[JsonProperty("integrationStepMeters")] public double IntegrationStepMeters { get; set; }
|
||||
[JsonProperty("maximumCollisionCheckStepMeters")] public double MaximumCollisionCheckStepMeters { get; set; }
|
||||
[JsonProperty("headingResolutionRadians")] public double HeadingResolutionRadians { get; set; }
|
||||
[JsonProperty("curvatureLevelCount")] public int CurvatureLevelCount { get; set; }
|
||||
[JsonProperty("goalPositionToleranceMeters")] public double GoalPositionToleranceMeters { get; set; }
|
||||
[JsonProperty("goalHeadingToleranceRadians")] public double GoalHeadingToleranceRadians { get; set; }
|
||||
[JsonProperty("maximumExpandedNodes")] public int MaximumExpandedNodes { get; set; }
|
||||
[JsonProperty("searchTimeoutSeconds")] public double SearchTimeoutSeconds { get; set; }
|
||||
[JsonProperty("heuristicWeight")] public double HeuristicWeight { get; set; }
|
||||
[JsonProperty("reverseCostMultiplier")] public double ReverseCostMultiplier { get; set; }
|
||||
[JsonProperty("gearSwitchPenaltyMeters")] public double GearSwitchPenaltyMeters { get; set; }
|
||||
[JsonProperty("curvatureMagnitudeWeight")] public double CurvatureMagnitudeWeight { get; set; }
|
||||
[JsonProperty("curvatureChangePenaltyMetersPerLevel")] public double CurvatureChangePenaltyMetersPerLevel { get; set; }
|
||||
[JsonProperty("clearanceCostWeight")] public double ClearanceCostWeight { get; set; }
|
||||
[JsonProperty("clearanceCostDistanceMeters")] public double ClearanceCostDistanceMeters { get; set; }
|
||||
[JsonProperty("allowReverse")] public bool AllowReverse { get; set; }
|
||||
}
|
||||
|
||||
internal sealed class SmoothingFixturePathPoint
|
||||
{
|
||||
[JsonProperty("xMeters")] public double XMeters { get; set; }
|
||||
[JsonProperty("yMeters")] public double YMeters { get; set; }
|
||||
[JsonProperty("headingRadians")] public double HeadingRadians { get; set; }
|
||||
[JsonProperty("unwrappedHeadingRadians")] public double UnwrappedHeadingRadians { get; set; }
|
||||
[JsonProperty("arcLengthMeters")] public double ArcLengthMeters { get; set; }
|
||||
[JsonProperty("direction")] public TravelDirection Direction { get; set; }
|
||||
[JsonProperty("vehicleCurvaturePerMeter")] public double VehicleCurvaturePerMeter { get; set; }
|
||||
[JsonProperty("bodyClearanceMeters")] public double BodyClearanceMeters { get; set; }
|
||||
[JsonProperty("isGearSwitchPoint")] public bool IsGearSwitchPoint { get; set; }
|
||||
[JsonProperty("source")] public CoarsePathPointSource Source { get; set; }
|
||||
}
|
||||
|
||||
internal sealed class SmoothingFixtureSegment
|
||||
{
|
||||
[JsonProperty("segmentIndex")] public int SegmentIndex { get; set; }
|
||||
[JsonProperty("direction")] public TravelDirection Direction { get; set; }
|
||||
[JsonProperty("startIndex")] public int StartIndex { get; set; }
|
||||
[JsonProperty("endIndex")] public int EndIndex { get; set; }
|
||||
[JsonProperty("startsAtGearSwitch")] public bool StartsAtGearSwitch { get; set; }
|
||||
[JsonProperty("endsAtGearSwitch")] public bool EndsAtGearSwitch { get; set; }
|
||||
}
|
||||
@@ -0,0 +1,244 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Globalization;
|
||||
using System.IO;
|
||||
using System.Security.Cryptography;
|
||||
using System.Text;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using Newtonsoft.Json;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Test;
|
||||
|
||||
/// <summary>加载并验证版本化 JSON 快速夹具；此类不运行 Hybrid A*。</summary>
|
||||
public static class SmoothingScenarioFixtureLoader
|
||||
{
|
||||
/// <summary>读取、校验并冻结指定的快速夹具文件。</summary>
|
||||
public static IReadOnlyList<SmoothingScenarioFixture> LoadAndVerify(string fixturePath)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(fixturePath)) throw new ArgumentException("夹具路径不能为空。", nameof(fixturePath));
|
||||
if (!File.Exists(fixturePath)) throw new FileNotFoundException("找不到路径平滑夹具文件。", fixturePath);
|
||||
|
||||
SmoothingFixtureDocument document = JsonConvert.DeserializeObject<SmoothingFixtureDocument>(
|
||||
File.ReadAllText(fixturePath, Encoding.UTF8));
|
||||
if (document == null || document.SchemaVersion != 1 || document.Scenarios == null)
|
||||
throw new InvalidDataException("路径平滑夹具架构版本无效。");
|
||||
|
||||
var fixtures = new List<SmoothingScenarioFixture>(document.Scenarios.Count);
|
||||
var ids = new HashSet<string>(StringComparer.Ordinal);
|
||||
for (int index = 0; index < document.Scenarios.Count; index++)
|
||||
{
|
||||
SmoothingFixtureRecord record = document.Scenarios[index];
|
||||
Validate(record, ids);
|
||||
string fingerprint = ComputeFingerprint(record);
|
||||
if (!string.Equals(record.ConfigurationFingerprint, fingerprint, StringComparison.Ordinal))
|
||||
throw new InvalidDataException("路径平滑夹具指纹已过期：" + record.Id + "。");
|
||||
fixtures.Add(new SmoothingScenarioFixture(record, true));
|
||||
}
|
||||
return fixtures.AsReadOnly();
|
||||
}
|
||||
|
||||
internal static string ComputeFingerprint(SmoothingFixtureRecord record)
|
||||
{
|
||||
string material = BuildFingerprintMaterial(record);
|
||||
using (SHA256 sha256 = SHA256.Create())
|
||||
{
|
||||
byte[] hash = sha256.ComputeHash(Encoding.UTF8.GetBytes(material));
|
||||
var builder = new StringBuilder(hash.Length * 2 + 7);
|
||||
builder.Append("sha256:");
|
||||
for (int index = 0; index < hash.Length; index++) builder.Append(hash[index].ToString("x2", CultureInfo.InvariantCulture));
|
||||
return builder.ToString();
|
||||
}
|
||||
}
|
||||
|
||||
internal static PlanningGridMap BuildMap(SmoothingScenarioFixture fixture)
|
||||
{
|
||||
SmoothingFixtureRecord record = fixture.Record;
|
||||
var obstacles = new List<IMapObstacle>(record.Map.Obstacles.Count);
|
||||
for (int index = 0; index < record.Map.Obstacles.Count; index++)
|
||||
{
|
||||
SmoothingFixtureObstacle obstacle = record.Map.Obstacles[index];
|
||||
if (string.Equals(obstacle.Kind, "circle", StringComparison.Ordinal))
|
||||
obstacles.Add(new CircleObstacle(obstacle.CenterXMm, obstacle.CenterYMm, obstacle.RadiusMm));
|
||||
else if (string.Equals(obstacle.Kind, "axis-aligned-rectangle", StringComparison.Ordinal))
|
||||
obstacles.Add(new AxisAlignedRectangleObstacle(obstacle.XMinMm, obstacle.XMaxMm, obstacle.YMinMm, obstacle.YMaxMm));
|
||||
else
|
||||
throw new InvalidDataException("夹具包含未知障碍物类型。");
|
||||
}
|
||||
|
||||
var mapRequest = new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(record.Map.XMinMm, record.Map.XMaxMm, record.Map.YMinMm, record.Map.YMaxMm),
|
||||
ResolutionMm = record.Map.ResolutionMm,
|
||||
AllowExplicitEmptyMap = obstacles.Count == 0,
|
||||
ObstacleSources = obstacles.Count == 0
|
||||
? Array.Empty<IMapObstacleSource>()
|
||||
: new IMapObstacleSource[] { new ManualObstacleSource("fixture-" + record.Id, record.FixtureVersion, true, obstacles) },
|
||||
};
|
||||
PlanningMapBuildResult build = new PlanningMapFactory().Create(mapRequest);
|
||||
if (!build.Succeeded || build.Map == null) throw new InvalidDataException("夹具地图无法重建：" + record.Id + "。");
|
||||
return build.Map;
|
||||
}
|
||||
|
||||
internal static VehicleParameters BuildVehicle(SmoothingScenarioFixture fixture)
|
||||
{
|
||||
SmoothingFixtureVehicle vehicle = fixture.Record.Vehicle;
|
||||
return new VehicleParameters
|
||||
{
|
||||
LengthMeters = vehicle.LengthMeters,
|
||||
WidthMeters = vehicle.WidthMeters,
|
||||
SafetyMarginMeters = vehicle.SafetyMarginMeters,
|
||||
MaximumCurvaturePerMeter = vehicle.MaximumCurvaturePerMeter,
|
||||
};
|
||||
}
|
||||
|
||||
private static void Validate(SmoothingFixtureRecord record, ISet<string> ids)
|
||||
{
|
||||
if (record == null || string.IsNullOrWhiteSpace(record.Id) || record.FixtureVersion <= 0 ||
|
||||
record.Map == null || record.Vehicle == null || record.PlanningConfiguration == null || record.Path == null || record.Path.Count < 2 ||
|
||||
record.Segments == null || record.Segments.Count == 0 || !ids.Add(record.Id))
|
||||
throw new InvalidDataException("路径平滑夹具缺少必需字段、版本或唯一 ID。");
|
||||
|
||||
ValidateMap(record);
|
||||
ValidateVehicle(record);
|
||||
ValidatePlanningConfiguration(record);
|
||||
ValidatePathContract(record);
|
||||
}
|
||||
|
||||
private static void ValidateMap(SmoothingFixtureRecord record)
|
||||
{
|
||||
SmoothingFixtureMap map = record.Map;
|
||||
if (!IsFinite(map.XMinMm) || !IsFinite(map.XMaxMm) || !IsFinite(map.YMinMm) || !IsFinite(map.YMaxMm) ||
|
||||
!IsFinite(map.ResolutionMm) || map.XMaxMm <= map.XMinMm || map.YMaxMm <= map.YMinMm || map.ResolutionMm <= 0f ||
|
||||
map.Obstacles == null)
|
||||
{
|
||||
throw new InvalidDataException("Fixture map contract is invalid: " + record.Id + ".");
|
||||
}
|
||||
|
||||
for (int index = 0; index < map.Obstacles.Count; index++)
|
||||
{
|
||||
SmoothingFixtureObstacle obstacle = map.Obstacles[index];
|
||||
bool circle = obstacle != null && string.Equals(obstacle.Kind, "circle", StringComparison.Ordinal) &&
|
||||
IsFinite(obstacle.CenterXMm) && IsFinite(obstacle.CenterYMm) && IsFinite(obstacle.RadiusMm) && obstacle.RadiusMm >= 0f;
|
||||
bool rectangle = obstacle != null && string.Equals(obstacle.Kind, "axis-aligned-rectangle", StringComparison.Ordinal) &&
|
||||
IsFinite(obstacle.XMinMm) && IsFinite(obstacle.XMaxMm) && IsFinite(obstacle.YMinMm) && IsFinite(obstacle.YMaxMm) &&
|
||||
obstacle.XMaxMm >= obstacle.XMinMm && obstacle.YMaxMm >= obstacle.YMinMm;
|
||||
if (!circle && !rectangle)
|
||||
throw new InvalidDataException("Fixture map contract is invalid: " + record.Id + ".");
|
||||
}
|
||||
}
|
||||
|
||||
private static void ValidateVehicle(SmoothingFixtureRecord record)
|
||||
{
|
||||
SmoothingFixtureVehicle vehicle = record.Vehicle;
|
||||
if (!IsFinite(vehicle.LengthMeters) || !IsFinite(vehicle.WidthMeters) || !IsFinite(vehicle.SafetyMarginMeters) ||
|
||||
!IsFinite(vehicle.MaximumCurvaturePerMeter) || vehicle.LengthMeters <= 0d || vehicle.WidthMeters <= 0d ||
|
||||
vehicle.SafetyMarginMeters < 0d || vehicle.MaximumCurvaturePerMeter <= 0d)
|
||||
{
|
||||
throw new InvalidDataException("Fixture vehicle contract is invalid: " + record.Id + ".");
|
||||
}
|
||||
}
|
||||
|
||||
private static void ValidatePlanningConfiguration(SmoothingFixtureRecord record)
|
||||
{
|
||||
SmoothingFixturePlanningConfiguration configuration = record.PlanningConfiguration;
|
||||
if (!IsPositiveFinite(configuration.PrimitiveLengthMeters) || !IsPositiveFinite(configuration.IntegrationStepMeters) ||
|
||||
!IsPositiveFinite(configuration.MaximumCollisionCheckStepMeters) || !IsPositiveFinite(configuration.HeadingResolutionRadians) ||
|
||||
configuration.CurvatureLevelCount < 2 || !IsPositiveFinite(configuration.GoalPositionToleranceMeters) ||
|
||||
!IsPositiveFinite(configuration.GoalHeadingToleranceRadians) || configuration.MaximumExpandedNodes <= 0 ||
|
||||
!IsPositiveFinite(configuration.SearchTimeoutSeconds) || !IsFinite(configuration.HeuristicWeight) ||
|
||||
!IsFinite(configuration.ReverseCostMultiplier) || !IsFinite(configuration.GearSwitchPenaltyMeters) ||
|
||||
!IsFinite(configuration.CurvatureMagnitudeWeight) || !IsFinite(configuration.CurvatureChangePenaltyMetersPerLevel) ||
|
||||
!IsFinite(configuration.ClearanceCostWeight) || !IsPositiveFinite(configuration.ClearanceCostDistanceMeters))
|
||||
{
|
||||
throw new InvalidDataException("Fixture planning configuration contract is invalid: " + record.Id + ".");
|
||||
}
|
||||
}
|
||||
|
||||
private static void ValidatePathContract(SmoothingFixtureRecord record)
|
||||
{
|
||||
var fixture = new SmoothingScenarioFixture(record, true);
|
||||
try
|
||||
{
|
||||
var request = new PathSmoothingRequest(
|
||||
fixture.Path,
|
||||
fixture.Segments,
|
||||
BuildMap(fixture),
|
||||
BuildVehicle(fixture),
|
||||
new PathSmoothingConfiguration());
|
||||
if (!new PathSmoothingPreprocessor().TryPrepare(request, out _, out string reason))
|
||||
throw new InvalidDataException("Fixture path contract is invalid: " + record.Id + "; " + reason);
|
||||
}
|
||||
catch (InvalidDataException)
|
||||
{
|
||||
throw;
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
throw new InvalidDataException("Fixture path contract is invalid: " + record.Id + "; " + exception.Message, exception);
|
||||
}
|
||||
}
|
||||
|
||||
private static bool IsFinite(float value)
|
||||
{
|
||||
return !float.IsNaN(value) && !float.IsInfinity(value);
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||||
}
|
||||
|
||||
private static bool IsPositiveFinite(double value)
|
||||
{
|
||||
return IsFinite(value) && value > 0d;
|
||||
}
|
||||
|
||||
private static string BuildFingerprintMaterial(SmoothingFixtureRecord record)
|
||||
{
|
||||
var builder = new StringBuilder();
|
||||
Append(builder, record.Id); Append(builder, record.FixtureVersion);
|
||||
Append(builder, record.Map.XMinMm); Append(builder, record.Map.XMaxMm); Append(builder, record.Map.YMinMm);
|
||||
Append(builder, record.Map.YMaxMm); Append(builder, record.Map.ResolutionMm);
|
||||
for (int index = 0; index < record.Map.Obstacles.Count; index++)
|
||||
{
|
||||
SmoothingFixtureObstacle obstacle = record.Map.Obstacles[index];
|
||||
Append(builder, obstacle.Kind); Append(builder, obstacle.XMinMm); Append(builder, obstacle.XMaxMm);
|
||||
Append(builder, obstacle.YMinMm); Append(builder, obstacle.YMaxMm); Append(builder, obstacle.CenterXMm);
|
||||
Append(builder, obstacle.CenterYMm); Append(builder, obstacle.RadiusMm);
|
||||
}
|
||||
Append(builder, record.Vehicle.LengthMeters); Append(builder, record.Vehicle.WidthMeters);
|
||||
Append(builder, record.Vehicle.SafetyMarginMeters); Append(builder, record.Vehicle.MaximumCurvaturePerMeter);
|
||||
SmoothingFixturePlanningConfiguration configuration = record.PlanningConfiguration;
|
||||
Append(builder, configuration.PrimitiveLengthMeters); Append(builder, configuration.IntegrationStepMeters);
|
||||
Append(builder, configuration.MaximumCollisionCheckStepMeters); Append(builder, configuration.HeadingResolutionRadians);
|
||||
Append(builder, configuration.CurvatureLevelCount); Append(builder, configuration.GoalPositionToleranceMeters);
|
||||
Append(builder, configuration.GoalHeadingToleranceRadians); Append(builder, configuration.MaximumExpandedNodes);
|
||||
Append(builder, configuration.SearchTimeoutSeconds); Append(builder, configuration.HeuristicWeight);
|
||||
Append(builder, configuration.ReverseCostMultiplier); Append(builder, configuration.GearSwitchPenaltyMeters);
|
||||
Append(builder, configuration.CurvatureMagnitudeWeight); Append(builder, configuration.CurvatureChangePenaltyMetersPerLevel);
|
||||
Append(builder, configuration.ClearanceCostWeight); Append(builder, configuration.ClearanceCostDistanceMeters);
|
||||
Append(builder, configuration.AllowReverse ? 1 : 0);
|
||||
for (int index = 0; index < record.Path.Count; index++)
|
||||
{
|
||||
SmoothingFixturePathPoint point = record.Path[index];
|
||||
Append(builder, point.XMeters); Append(builder, point.YMeters); Append(builder, point.HeadingRadians);
|
||||
Append(builder, point.UnwrappedHeadingRadians); Append(builder, point.ArcLengthMeters); Append(builder, (int)point.Direction);
|
||||
Append(builder, point.VehicleCurvaturePerMeter); Append(builder, point.BodyClearanceMeters);
|
||||
Append(builder, point.IsGearSwitchPoint ? 1 : 0); Append(builder, (int)point.Source);
|
||||
}
|
||||
for (int index = 0; index < record.Segments.Count; index++)
|
||||
{
|
||||
SmoothingFixtureSegment segment = record.Segments[index];
|
||||
Append(builder, segment.SegmentIndex); Append(builder, (int)segment.Direction); Append(builder, segment.StartIndex);
|
||||
Append(builder, segment.EndIndex); Append(builder, segment.StartsAtGearSwitch ? 1 : 0); Append(builder, segment.EndsAtGearSwitch ? 1 : 0);
|
||||
}
|
||||
return builder.ToString();
|
||||
}
|
||||
|
||||
private static void Append(StringBuilder builder, string value) { builder.Append(value ?? string.Empty).Append('|'); }
|
||||
private static void Append(StringBuilder builder, int value) { builder.Append(value.ToString(CultureInfo.InvariantCulture)).Append('|'); }
|
||||
private static void Append(StringBuilder builder, float value) { builder.Append(value.ToString("R", CultureInfo.InvariantCulture)).Append('|'); }
|
||||
private static void Append(StringBuilder builder, double value) { builder.Append(value.ToString("R", CultureInfo.InvariantCulture)).Append('|'); }
|
||||
}
|
||||
@@ -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,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,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.'
|
||||
Reference in New Issue
Block a user