109 lines
4.4 KiB
C#
109 lines
4.4 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Collections.ObjectModel;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
|
|
|
/// <summary>路径平滑的最终不可变结果。</summary>
|
|
public sealed class PathSmoothingResult
|
|
{
|
|
private static readonly IReadOnlyList<SmoothedPathPoint> EmptyPath =
|
|
new ReadOnlyCollection<SmoothedPathPoint>(new List<SmoothedPathPoint>());
|
|
private static readonly IReadOnlyList<SmoothedPathSegment> EmptySegments =
|
|
new ReadOnlyCollection<SmoothedPathSegment>(new List<SmoothedPathSegment>());
|
|
|
|
private PathSmoothingResult(
|
|
PathSmoothingStatus status,
|
|
SmoothingMethod? method,
|
|
IReadOnlyList<SmoothedPathPoint> path,
|
|
IReadOnlyList<SmoothedPathSegment> segments,
|
|
PathSmoothingDiagnostics diagnostics)
|
|
{
|
|
Status = status;
|
|
Method = method;
|
|
Path = path;
|
|
Segments = segments;
|
|
Diagnostics = diagnostics ?? new PathSmoothingDiagnostics(
|
|
new PathQualityMetrics(),
|
|
TimeSpan.Zero,
|
|
0,
|
|
0d,
|
|
"No smoothing diagnostics were supplied.");
|
|
}
|
|
|
|
/// <summary>最终发布状态。</summary>
|
|
public PathSmoothingStatus Status { get; }
|
|
|
|
/// <summary>成功或回退时的实际（或尝试）平滑方法；失败时为空。</summary>
|
|
public SmoothingMethod? Method { get; }
|
|
|
|
/// <summary>成功或经过复核的回退路径；其他状态始终为空且不可变。</summary>
|
|
public IReadOnlyList<SmoothedPathPoint> Path { get; }
|
|
|
|
/// <summary>覆盖 <see cref="Path"/> 的方向分段；其他状态始终为空且不可变。</summary>
|
|
public IReadOnlyList<SmoothedPathSegment> Segments { get; }
|
|
|
|
/// <summary>本次平滑的质量和终止诊断；始终非空。</summary>
|
|
public PathSmoothingDiagnostics Diagnostics { get; }
|
|
|
|
/// <summary>创建已通过所有复核的平滑结果。</summary>
|
|
public static PathSmoothingResult Success(
|
|
SmoothingMethod method,
|
|
IReadOnlyList<SmoothedPathPoint> path,
|
|
IReadOnlyList<SmoothedPathSegment> segments,
|
|
PathSmoothingDiagnostics diagnostics)
|
|
{
|
|
ValidatePublishedPath(path, segments);
|
|
return new PathSmoothingResult(
|
|
PathSmoothingStatus.Success,
|
|
method,
|
|
CopyReadOnly(path),
|
|
CopyReadOnly(segments),
|
|
diagnostics);
|
|
}
|
|
|
|
/// <summary>创建经过完整复核的原始粗路径回退结果。</summary>
|
|
public static PathSmoothingResult Fallback(
|
|
SmoothingMethod attemptedMethod,
|
|
IReadOnlyList<SmoothedPathPoint> path,
|
|
IReadOnlyList<SmoothedPathSegment> segments,
|
|
PathSmoothingDiagnostics diagnostics)
|
|
{
|
|
ValidatePublishedPath(path, segments);
|
|
return new PathSmoothingResult(
|
|
PathSmoothingStatus.FallbackToCoarsePath,
|
|
attemptedMethod,
|
|
CopyReadOnly(path),
|
|
CopyReadOnly(segments),
|
|
diagnostics);
|
|
}
|
|
|
|
/// <summary>创建不发布路径的失败、不可行、取消或输入无效结果。</summary>
|
|
public static PathSmoothingResult Failure(PathSmoothingStatus status, PathSmoothingDiagnostics diagnostics)
|
|
{
|
|
if (status == PathSmoothingStatus.Success || status == PathSmoothingStatus.FallbackToCoarsePath)
|
|
throw new ArgumentException("Use Success or Fallback to publish a path.", nameof(status));
|
|
if (!Enum.IsDefined(typeof(PathSmoothingStatus), status))
|
|
throw new ArgumentOutOfRangeException(nameof(status));
|
|
return new PathSmoothingResult(status, null, EmptyPath, EmptySegments, diagnostics);
|
|
}
|
|
|
|
private static void ValidatePublishedPath(
|
|
IReadOnlyList<SmoothedPathPoint> path,
|
|
IReadOnlyList<SmoothedPathSegment> segments)
|
|
{
|
|
if (path == null || path.Count == 0)
|
|
throw new ArgumentException("Published smoothing results require a non-empty path.", nameof(path));
|
|
if (segments == null || segments.Count == 0)
|
|
throw new ArgumentException("Published smoothing results require non-empty segments.", nameof(segments));
|
|
}
|
|
|
|
private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
|
|
{
|
|
var copy = new List<T>(source.Count);
|
|
for (int index = 0; index < source.Count; index++)
|
|
copy.Add(source[index]);
|
|
return new ReadOnlyCollection<T>(copy);
|
|
}
|
|
}
|