245 lines
14 KiB
C#
245 lines
14 KiB
C#
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('|'); }
|
|
}
|