157 lines
8.4 KiB
C#
157 lines
8.4 KiB
C#
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; }
|
|
}
|