chore: save current workspace progress
This commit is contained in:
梁薄云
+19451
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,161 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Threading;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath.Test;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Comparison;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Facade;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Visualization;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Test;
|
||||
|
||||
/// <summary>开发人员离线导出八个快速夹具和四个 Hybrid A* 端到端场景的比较报告。</summary>
|
||||
public sealed class PathSmoothingComparisonDemo
|
||||
{
|
||||
private readonly PathSmoothingService _smoothingService = new PathSmoothingService();
|
||||
private readonly PathSmoothingComparisonService _comparisonService = new PathSmoothingComparisonService();
|
||||
private readonly SmoothingFigureModelBuilder _figureBuilder = new SmoothingFigureModelBuilder();
|
||||
private readonly SmoothingReportExporter _reportExporter = new SmoothingReportExporter();
|
||||
|
||||
/// <summary>导出八个已验证夹具；不运行 Hybrid A*。</summary>
|
||||
public IReadOnlyList<PathSmoothingComparisonScenarioResult> ExportFixtureReports(
|
||||
string fixturePath,
|
||||
string outputDirectory,
|
||||
CancellationToken cancellationToken = default)
|
||||
{
|
||||
IReadOnlyList<SmoothingScenarioFixture> fixtures = SmoothingScenarioFixtureLoader.LoadAndVerify(fixturePath);
|
||||
IReadOnlyList<PathSmoothingComparisonRequest> requests = SmoothingScenarioFactory.CreateFixtureRequests(fixturePath);
|
||||
var results = new List<PathSmoothingComparisonScenarioResult>(requests.Count);
|
||||
for (int index = 0; index < requests.Count; index++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
results.Add(Export(fixtures[index].Id, requests[index], outputDirectory, cancellationToken));
|
||||
}
|
||||
return results.AsReadOnly();
|
||||
}
|
||||
|
||||
/// <summary>运行四个固定 Hybrid A* 场景，并仅为成功粗路径导出平滑比较报告。</summary>
|
||||
public IReadOnlyList<PathSmoothingComparisonScenarioResult> ExportEndToEndReports(
|
||||
string outputDirectory,
|
||||
CancellationToken cancellationToken = default)
|
||||
{
|
||||
CoarsePathTestScenario[] scenarios =
|
||||
{
|
||||
CoarsePathTestScenario.ExplicitEmpty,
|
||||
CoarsePathTestScenario.RectangleDetour,
|
||||
CoarsePathTestScenario.ManualAndTwoLeg,
|
||||
CoarsePathTestScenario.ReverseGearSwitch,
|
||||
};
|
||||
var planner = new CoarsePathPlanningService();
|
||||
var results = new List<PathSmoothingComparisonScenarioResult>(scenarios.Length);
|
||||
for (int index = 0; index < scenarios.Length; index++)
|
||||
{
|
||||
cancellationToken.ThrowIfCancellationRequested();
|
||||
CoarsePathPlanningJob job = CoarsePathScenarioFactory.Create(scenarios[index]);
|
||||
CoarsePathPlanningJobResult coarse = planner.Plan(job, cancellationToken);
|
||||
if (coarse.PlanningResult.Status != PlanningStatus.Success)
|
||||
{
|
||||
results.Add(PathSmoothingComparisonScenarioResult.CoarsePathFailure(
|
||||
scenarios[index].ToString(), coarse.PlanningResult.Status, coarse.PlanningResult.Diagnostics.TerminationReason));
|
||||
continue;
|
||||
}
|
||||
results.Add(Export(
|
||||
scenarios[index].ToString(),
|
||||
SmoothingScenarioFactory.CreateEndToEndRequest(job, coarse),
|
||||
outputDirectory,
|
||||
cancellationToken));
|
||||
}
|
||||
return results.AsReadOnly();
|
||||
}
|
||||
|
||||
/// <summary>供业务调用示例使用：仅在已成功的粗路径上调用平滑服务。</summary>
|
||||
public PathSmoothingResult SmoothSuccessfulCoarsePath(
|
||||
CoarsePathPlanningJob job,
|
||||
CoarsePathPlanningJobResult coarseResult,
|
||||
PathSmoothingConfiguration configuration,
|
||||
CancellationToken cancellationToken = default)
|
||||
{
|
||||
if (job == null || coarseResult == null || coarseResult.PlanningResult.Status != PlanningStatus.Success)
|
||||
throw new ArgumentException("只有成功的粗路径能够进入平滑流程。", nameof(coarseResult));
|
||||
return _smoothingService.Smooth(new PathSmoothingRequest(
|
||||
coarseResult.PlanningResult.Path,
|
||||
coarseResult.PlanningResult.Segments,
|
||||
coarseResult.MapResult.Map,
|
||||
job.Vehicle,
|
||||
configuration), cancellationToken);
|
||||
}
|
||||
|
||||
/// <summary>业务调用方只可消费成功平滑或显式回退的路径。</summary>
|
||||
public static bool IsConsumable(PathSmoothingStatus status)
|
||||
{
|
||||
return status == PathSmoothingStatus.Success || status == PathSmoothingStatus.FallbackToCoarsePath;
|
||||
}
|
||||
|
||||
private PathSmoothingComparisonScenarioResult Export(
|
||||
string scenarioId,
|
||||
PathSmoothingComparisonRequest request,
|
||||
string outputDirectory,
|
||||
CancellationToken cancellationToken)
|
||||
{
|
||||
if (request == null || request.SmoothingRequest == null || request.SmoothingRequest.CoarsePath.Count == 0)
|
||||
throw new ArgumentException("比较请求必须包含粗路径。", nameof(request));
|
||||
PathSmoothingComparisonResult comparison = _comparisonService.Compare(request, cancellationToken);
|
||||
CoarsePathPoint first = request.SmoothingRequest.CoarsePath[0];
|
||||
CoarsePathPoint last = request.SmoothingRequest.CoarsePath[request.SmoothingRequest.CoarsePath.Count - 1];
|
||||
SmoothingFigureModel model = _figureBuilder.Build(
|
||||
comparison,
|
||||
request.SmoothingRequest.Map,
|
||||
new Pose2D(first.X, first.Y, first.Heading),
|
||||
new Pose2D(last.X, last.Y, last.Heading),
|
||||
scenarioId,
|
||||
scenarioId);
|
||||
SmoothingReportExportResult report = _reportExporter.Export(new SmoothingReportExportRequest
|
||||
{
|
||||
Model = model,
|
||||
OutputDirectory = System.IO.Path.Combine(outputDirectory, scenarioId),
|
||||
FileStem = "comparison",
|
||||
});
|
||||
return PathSmoothingComparisonScenarioResult.ComparisonComplete(scenarioId, comparison, report);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>批处理为一个场景发布的粗路径状态、平滑比较和报告结果。</summary>
|
||||
public sealed class PathSmoothingComparisonScenarioResult
|
||||
{
|
||||
private PathSmoothingComparisonScenarioResult(
|
||||
string scenarioId,
|
||||
PlanningStatus coarsePathStatus,
|
||||
string diagnostic,
|
||||
PathSmoothingComparisonResult comparison,
|
||||
SmoothingReportExportResult report)
|
||||
{
|
||||
ScenarioId = scenarioId ?? string.Empty;
|
||||
CoarsePathStatus = coarsePathStatus;
|
||||
Diagnostic = diagnostic ?? string.Empty;
|
||||
Comparison = comparison;
|
||||
Report = report;
|
||||
}
|
||||
|
||||
public string ScenarioId { get; }
|
||||
public PlanningStatus CoarsePathStatus { get; }
|
||||
public string Diagnostic { get; }
|
||||
public PathSmoothingComparisonResult Comparison { get; }
|
||||
public SmoothingReportExportResult Report { get; }
|
||||
|
||||
internal static PathSmoothingComparisonScenarioResult CoarsePathFailure(
|
||||
string scenarioId,
|
||||
PlanningStatus status,
|
||||
string diagnostic)
|
||||
{
|
||||
return new PathSmoothingComparisonScenarioResult(scenarioId, status, diagnostic, null, null);
|
||||
}
|
||||
|
||||
internal static PathSmoothingComparisonScenarioResult ComparisonComplete(
|
||||
string scenarioId,
|
||||
PathSmoothingComparisonResult comparison,
|
||||
SmoothingReportExportResult report)
|
||||
{
|
||||
return new PathSmoothingComparisonScenarioResult(scenarioId, PlanningStatus.Success, string.Empty, comparison, report);
|
||||
}
|
||||
}
|
||||
@@ -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,258 @@
|
||||
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)
|
||||
{
|
||||
AppendBits(builder, BitConverter.GetBytes(value));
|
||||
}
|
||||
|
||||
private static void Append(StringBuilder builder, double value)
|
||||
{
|
||||
AppendBits(builder, BitConverter.GetBytes(value));
|
||||
}
|
||||
|
||||
private static void AppendBits(StringBuilder builder, byte[] bytes)
|
||||
{
|
||||
for (int index = bytes.Length - 1; index >= 0; index--)
|
||||
builder.Append(bytes[index].ToString("x2", CultureInfo.InvariantCulture));
|
||||
builder.Append('|');
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user