First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

chore: save current workspace progress

Browse Source
This commit is contained in:
梁薄云
2026-08-09 22:13:18 +08:00
parent 650c2ab0e3
commit 2f4fd15e52
449 changed files with 76593 additions and 971 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/CubicBSplineOptions.cs
-8
View File
@@ -1,8 +0,0 @@
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
/// <summary>三次 B 样条平滑参数。</summary>
public sealed class CubicBSplineOptions
{
/// <summary>端点切向控制柄相对于相邻弦长的比例。</summary>
public double EndpointTangentScale { get; set; } = 1d / 3d;
}
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/LocalCubicBezierOptions.cs
-16
View File
@@ -1,16 +0,0 @@
using System;
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
/// <summary>局部三次 Bézier 平滑参数。</summary>
public sealed class LocalCubicBezierOptions
{
/// <summary>判定为明显转角的最小航向变化，单位 rad。</summary>
public double CornerHeadingThresholdRadians { get; set; } = Math.PI / 18d;
/// <summary>单个局部平滑窗口的最大弧长，单位 m。</summary>
public double MaximumWindowLengthMeters { get; set; } = 0.60d;
/// <summary>控制柄相对于窗口局部弦长的比例。</summary>
public double HandleLengthRatio { get; set; } = 1d / 3d;
}
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingConfiguration.cs
+11 -35
View File
@@ -1,53 +1,29 @@
using System.Collections.Generic;
using System.Collections.ObjectModel;
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
/// <summary>路径平滑的公共配置；所有距离使用 m。</summary>
/// <summary>Local G2 路径平滑管线共享配置；所有距离单位为 m，曲率相关限制使用 1/m。</summary>
public sealed class PathSmoothingConfiguration
{
/// <summary>创建带有安全默认值的平滑配置。</summary>
/// <summary>创建采用首版安全采样、碰撞步长、净空和曲率容差默认值的可编辑配置。</summary>
public PathSmoothingConfiguration()
{
OutputSpacingMeters = 0.05d;
OutputSpacingMeters = 0.025d;
MaximumCollisionCheckStepMeters = 0.025d;
MinimumClearanceReserveMeters = 0.02d;
SmoothingStrength = 1d;
AllowFallbackToCoarsePath = true;
RetryStrengthScales = new ReadOnlyCollection<double>(
new List<double> { 1d, 0.75d, 0.50d, 0.25d });
MinimumClearanceReserveMeters = 0d;
CurvatureLimitRadiusToleranceMeters = 0.002d;
}
/// <summary>正式单算法入口使用的方法。</summary>
public SmoothingMethod Method { get; set; }
/// <summary>输出路径的目标弧长采样间距，单位 m。</summary>
/// <summary>发布路径沿弧长的目标采样间距，单位 m；必须为有限正数。</summary>
public double OutputSpacingMeters { get; set; }
/// <summary>扫掠碰撞检查的最大步长，单位 m。</summary>
/// <summary>连续车体扫掠复核允许的最大中心步长，单位 m；较大值会降低碰撞检查分辨率。</summary>
public double MaximumCollisionCheckStepMeters { get; set; }
/// <summary>平滑候选必须在最小净空之外保留的额外余量，单位 m。</summary>
/// <summary>除无碰撞外还要求保留的最小额外净空，单位 m。</summary>
public double MinimumClearanceReserveMeters { get; set; }
/// <summary>算法初始平滑强度。</summary>
public double SmoothingStrength { get; set; }
/// <summary>曲率上限复核时允许相对名义最小转弯半径的缩减容差，单位 m。</summary>
public double CurvatureLimitRadiusToleranceMeters { get; set; }
/// <summary>所有平滑尝试失败时是否允许发布经过复核的原粗路径。</summary>
public bool AllowFallbackToCoarsePath { get; set; }
/// <summary>有限且严格递减的平滑强度重试比例。</summary>
public IReadOnlyList<double> RetryStrengthScales { get; }
/// <summary>三次 B 样条专用参数。</summary>
public CubicBSplineOptions CubicBSpline { get; } = new CubicBSplineOptions();
/// <summary>局部三次 Bézier 专用参数。</summary>
public LocalCubicBezierOptions LocalCubicBezier { get; } = new LocalCubicBezierOptions();
/// <summary>分段五次多项式专用参数。</summary>
public PiecewiseQuinticOptions PiecewiseQuintic { get; } = new PiecewiseQuinticOptions();
/// <summary>局部 G2 五次过渡专用参数。</summary>
/// <summary>局部 G2 五次过渡窗口与候选筛选选项；返回同一配置对象的可编辑子配置。</summary>
public LocalG2QuinticOptions LocalG2Quintic { get; } = new LocalG2QuinticOptions();
}
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingDiagnostics.cs
+10 -54
View File
@@ -2,79 +2,35 @@ using System;
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
/// <summary>一次平滑尝试的不可变诊断信息。</summary>
/// <summary>一次 Local G2 平滑尝试的不可变诊断快照，适用于成功、取消和失败结果。</summary>
public sealed class PathSmoothingDiagnostics
{
/// <summary>创建不含路径指标的默认诊断信息。</summary>
/// <summary>创建不可行、零耗时且没有终止原因的默认诊断快照。</summary>
public PathSmoothingDiagnostics()
: this(new PathQualityMetrics(), TimeSpan.Zero, 0, 0d, string.Empty)
: this(new PathQualityMetrics(), TimeSpan.Zero, string.Empty)
{
}
/// <summary>创建完整的平滑诊断快照。</summary>
/// <summary>创建平滑诊断快照。</summary>
/// <param name="metrics">路径可行性、曲率、净空和相对变化的质量指标。</param>
/// <param name="elapsed">从服务入口到终止的累计耗时。</param>
/// <param name="terminationReason">可供调用方记录的成功、取消或失败原因；为 <see langword="null"/> 时为空字符串。</param>
public PathSmoothingDiagnostics(
PathQualityMetrics metrics,
TimeSpan elapsed,
int retryCount,
double acceptedStrength,
string terminationReason = null)
{
Metrics = metrics ?? new PathQualityMetrics();
Elapsed = elapsed;
RetryCount = retryCount;
AcceptedStrength = acceptedStrength;
TerminationReason = terminationReason ?? string.Empty;
}
/// <summary>使用所有测量量创建平滑诊断快照。</summary>
public PathSmoothingDiagnostics(
bool isFeasible,
double pathLengthMeters,
double maximumAbsoluteVehicleCurvaturePerMeter,
double rootMeanSquareVehicleCurvaturePerMeter,
double totalAbsoluteCurvatureVariationPerMeter,
double curvatureVariationEnergy,
double minimumBodyClearanceMeters,
double lengthChangePercent,
double peakCurvatureChangePercent,
double curvatureVariationChangePercent,
double minimumClearanceChangeMeters,
TimeSpan elapsed,
int retryCount,
double acceptedStrength,
string terminationReason = null)
: this(
new PathQualityMetrics(
isFeasible,
pathLengthMeters,
maximumAbsoluteVehicleCurvaturePerMeter,
rootMeanSquareVehicleCurvaturePerMeter,
totalAbsoluteCurvatureVariationPerMeter,
curvatureVariationEnergy,
minimumBodyClearanceMeters,
lengthChangePercent,
peakCurvatureChangePercent,
curvatureVariationChangePercent,
minimumClearanceChangeMeters),
elapsed,
retryCount,
acceptedStrength,
terminationReason)
{
}
/// <summary>路径质量指标；始终非空。</summary>
/// <summary>本次尝试的质量指标；未提供时为全零且不可行的默认指标。</summary>
public PathQualityMetrics Metrics { get; }
/// <summary>从算法入口到返回诊断的耗时。</summary>
/// <summary>本次尝试的累计耗时，类型为 <see cref="TimeSpan"/>。</summary>
public TimeSpan Elapsed { get; }
/// <summary>已执行的安全强度重试次数。</summary>
public int RetryCount { get; }
/// <summary>通过复核的平滑强度；未接受候选时为零。</summary>
public double AcceptedStrength { get; }
/// <summary>面向调用方的稳定终止说明。</summary>
/// <summary>终止原因文本；不承载可消费的部分路径。</summary>
public string TerminationReason { get; }
}
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRequest.cs
+16 -14
View File
@@ -5,13 +5,23 @@ using MultiWheelC.TrajectoryPlanning.Mapping;
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
/// <summary>路径平滑所需的原始粗路径、复核上下文和配置。</summary>
/// <summary>
/// 一次路径平滑所需的不可变输入快照。
/// 粗路径、方向段、车辆和配置均在构造时复制；地图保留为调用方已冻结的 <see cref="PlanningGridMap"/> 快照。
/// </summary>
public sealed class PathSmoothingRequest
{
private readonly VehicleParameters _vehicle;
private readonly PathSmoothingConfiguration _configuration;
/// <summary>创建路径平滑请求，并复制粗路径和方向分段集合。</summary>
/// <summary>
/// 创建路径平滑请求，并防御性复制可变的路径、分段、车辆和配置输入。
/// </summary>
/// <param name="coarsePath">按起点到终点顺序排列的粗路径点集合；位置与弧长单位为 m，航向单位为 rad。</param>
/// <param name="segments">与 <paramref name="coarsePath"/> 对应的前进/倒车包含式方向分段集合。</param>
/// <param name="map">用于完整车体碰撞与净空复核的已准备不可变规划地图快照。</param>
/// <param name="vehicle">车辆长宽、安全余量和曲率约束；长度单位为 m，曲率单位为 1/m。</param>
/// <param name="configuration">本次平滑的采样、净空、曲率和 Local G2 配置；构造后不会与调用方共享可变实例。</param>
public PathSmoothingRequest(
IReadOnlyList<CoarsePathPoint> coarsePath,
IReadOnlyList<PathSegment> segments,
@@ -26,13 +36,13 @@ public sealed class PathSmoothingRequest
_configuration = CopyConfiguration(configuration);
}
/// <summary>原始粗路径的不可变快照。</summary>
/// <summary>原始粗路径的只读快照，按起点到终点顺序排列；位置与弧长单位为 m，航向单位为 rad。</summary>
public IReadOnlyList<CoarsePathPoint> CoarsePath { get; }
/// <summary>原始粗路径方向分段的不可变快照。</summary>
/// <summary>原始粗路径方向分段的只读快照；每段标识连续前进或倒车区间。</summary>
public IReadOnlyList<PathSegment> Segments { get; }
/// <summary>用于平滑后完整车体复核的规划栅格地图。</summary>
/// <summary>用于平滑后完整车体碰撞和净空复核的已冻结规划栅格地图快照。</summary>
public PlanningGridMap Map { get; }
/// <summary>车辆几何与最大曲率约束的不可变快照副本。</summary>
@@ -70,19 +80,11 @@ public sealed class PathSmoothingRequest
if (source == null) return null;
var copy = new PathSmoothingConfiguration
{
Method = source.Method,
OutputSpacingMeters = source.OutputSpacingMeters,
MaximumCollisionCheckStepMeters = source.MaximumCollisionCheckStepMeters,
MinimumClearanceReserveMeters = source.MinimumClearanceReserveMeters,
SmoothingStrength = source.SmoothingStrength,
AllowFallbackToCoarsePath = source.AllowFallbackToCoarsePath,
CurvatureLimitRadiusToleranceMeters = source.CurvatureLimitRadiusToleranceMeters,
};
copy.CubicBSpline.EndpointTangentScale = source.CubicBSpline.EndpointTangentScale;
copy.LocalCubicBezier.CornerHeadingThresholdRadians = source.LocalCubicBezier.CornerHeadingThresholdRadians;
copy.LocalCubicBezier.MaximumWindowLengthMeters = source.LocalCubicBezier.MaximumWindowLengthMeters;
copy.LocalCubicBezier.HandleLengthRatio = source.LocalCubicBezier.HandleLengthRatio;
copy.PiecewiseQuintic.KnotSpacingMeters = source.PiecewiseQuintic.KnotSpacingMeters;
copy.PiecewiseQuintic.MinimumKnotSpacingMeters = source.PiecewiseQuintic.MinimumKnotSpacingMeters;
copy.LocalG2Quintic.MinimumWindowLengthMeters = source.LocalG2Quintic.MinimumWindowLengthMeters;
copy.LocalG2Quintic.PreferredWindowLengthMeters = source.LocalG2Quintic.PreferredWindowLengthMeters;
copy.LocalG2Quintic.MaximumWindowLengthMeters = source.LocalG2Quintic.MaximumWindowLengthMeters;
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingResult.cs
+29 -55
View File
@@ -4,7 +4,10 @@ using System.Collections.ObjectModel;
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
/// <summary>路径平滑的最终不可变结果。</summary>
/// <summary>
/// Local G2 路径平滑管线产生的不可变结果。
/// 只有可发布状态携带完整、已验证的路径和方向段；失败、取消和无效输入结果始终提供空集合，不能被当作部分路径消费。
/// </summary>
public sealed class PathSmoothingResult
{
private static readonly IReadOnlyList<SmoothedPathPoint> EmptyPath =
@@ -30,64 +33,34 @@ public sealed class PathSmoothingResult
Diagnostics = diagnostics ?? new PathSmoothingDiagnostics(
new PathQualityMetrics(),
TimeSpan.Zero,
0,
0d,
"No smoothing diagnostics were supplied.");
}
/// <summary>最终发布状态。</summary>
/// <summary>本次平滑的终止状态；决定 <see cref="Path"/> 和 <see cref="Segments"/> 是否可消费。</summary>
public PathSmoothingStatus Status { get; }
/// <summary>成功或回退时的实际（或尝试）平滑方法；失败时为空。</summary>
/// <summary>成功发布路径时实际采用的平滑方法；失败结果为 <see langword="null"/>。</summary>
public SmoothingMethod? Method { get; }
/// <summary>成功或经过复核的回退路径；其他状态始终为空且不可变。</summary>
/// <summary>成功时按起点到终点排列的不可变平滑路径；位置和弧长单位为 m，航向单位为 rad；失败时为空。</summary>
public IReadOnlyList<SmoothedPathPoint> Path { get; }
/// <summary>覆盖 <see cref="Path"/> 的方向分段；其他状态始终为空且不可变。</summary>
/// <summary>成功时覆盖 <see cref="Path"/> 的不可变方向段集合；失败时为空。</summary>
public IReadOnlyList<SmoothedPathSegment> Segments { get; }
/// <summary>局部 G2 各检测区域的不可变报告；传统算法结果为空。</summary>
/// <summary>各 Local G2 区域的不可变处理报告；失败结果不包含区域发布记录。</summary>
public IReadOnlyList<PathSmoothingRegionReport> RegionReports { get; }
/// <summary>本次平滑的质量和终止诊断；始终非空。</summary>
/// <summary>质量指标、耗时和终止原因；始终存在，供调用方诊断成功或失败。</summary>
public PathSmoothingDiagnostics Diagnostics { get; }
/// <summary>创建已通过所有复核的平滑结果。</summary>
public static PathSmoothingResult Success(
SmoothingMethod method,
IReadOnlyList<SmoothedPathPoint> path,
IReadOnlyList<SmoothedPathSegment> segments,
PathSmoothingDiagnostics diagnostics)
{
ValidatePublishedResult(method, path, segments, diagnostics);
return new PathSmoothingResult(
PathSmoothingStatus.Success,
method,
CopyReadOnly(path),
CopyReadOnly(segments),
diagnostics,
EmptyRegionReports);
}
/// <summary>创建经过完整复核的原始粗路径回退结果。</summary>
public static PathSmoothingResult Fallback(
SmoothingMethod attemptedMethod,
IReadOnlyList<SmoothedPathPoint> path,
IReadOnlyList<SmoothedPathSegment> segments,
PathSmoothingDiagnostics diagnostics)
{
ValidatePublishedResult(attemptedMethod, path, segments, diagnostics);
return new PathSmoothingResult(
PathSmoothingStatus.FallbackToCoarsePath,
attemptedMethod,
CopyReadOnly(path),
CopyReadOnly(segments),
diagnostics,
EmptyRegionReports);
}
/// <summary>发布经过完整复核的局部 G2 预平滑结果。</summary>
/// <summary>发布已经过独立几何、曲率、净空和连续碰撞复核的 Local G2 结果。</summary>
/// <param name="status">可发布状态，只能是完整、部分改进、不需要平滑或保持原样之一。</param>
/// <param name="path">按起点到终点顺序排列的完整平滑路径；位置与弧长单位为 m，航向单位为 rad。</param>
/// <param name="segments">覆盖完整路径的前进/倒车方向段集合。</param>
/// <param name="diagnostics">包含可行质量指标和终止说明的诊断快照。</param>
/// <param name="regionReports">每个局部区域的不可变处理报告集合，不能为 <see langword="null"/>。</param>
/// <returns>携带防御性复制路径、分段和区域报告的不可变可消费结果。</returns>
public static PathSmoothingResult PublishLocalG2(
PathSmoothingStatus status,
IReadOnlyList<SmoothedPathPoint> path,
@@ -99,11 +72,13 @@ public sealed class PathSmoothingResult
status != PathSmoothingStatus.PartialImprovement &&
status != PathSmoothingStatus.NotNeeded &&
status != PathSmoothingStatus.Unchanged)
{
throw new ArgumentException("Use a Local G2 publication status.", nameof(status));
}
if (regionReports == null)
throw new ArgumentNullException(nameof(regionReports));
ValidatePublishedResult(SmoothingMethod.LocalG2Quintic, path, segments, diagnostics);
ValidatePublishedResult(path, segments, diagnostics);
return new PathSmoothingResult(
status,
SmoothingMethod.LocalG2Quintic,
@@ -113,29 +88,29 @@ public sealed class PathSmoothingResult
CopyReadOnly(regionReports));
}
/// <summary>创建不发布路径的失败、不可行、取消或输入无效结果。</summary>
/// <summary>创建明确不发布路径的失败、取消或无效输入结果。</summary>
/// <param name="status">非可发布的终止状态。</param>
/// <param name="diagnostics">失败原因、质量指标和耗时；为 <see langword="null"/> 时生成默认诊断。</param>
/// <returns>路径、方向段和区域报告均为空的不可变结果，不能作为部分路径使用。</returns>
public static PathSmoothingResult Failure(PathSmoothingStatus status, PathSmoothingDiagnostics diagnostics)
{
if (status == PathSmoothingStatus.Success ||
status == PathSmoothingStatus.FallbackToCoarsePath ||
status == PathSmoothingStatus.Complete ||
if (status == PathSmoothingStatus.Complete ||
status == PathSmoothingStatus.PartialImprovement ||
status == PathSmoothingStatus.NotNeeded ||
status == PathSmoothingStatus.Unchanged)
throw new ArgumentException("Use Success or Fallback to publish a path.", nameof(status));
{
throw new ArgumentException("Use PublishLocalG2 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, EmptyRegionReports);
}
private static void ValidatePublishedResult(
SmoothingMethod method,
IReadOnlyList<SmoothedPathPoint> path,
IReadOnlyList<SmoothedPathSegment> segments,
PathSmoothingDiagnostics diagnostics)
{
if (!Enum.IsDefined(typeof(SmoothingMethod), method))
throw new ArgumentOutOfRangeException(nameof(method));
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)
@@ -147,8 +122,7 @@ public sealed class PathSmoothingResult
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]);
for (int index = 0; index < source.Count; index++) copy.Add(source[index]);
return new ReadOnlyCollection<T>(copy);
}
}
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingStatus.cs
+1 -3
View File
@@ -1,10 +1,8 @@
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
/// <summary>路径平滑的最终发布状态。</summary>
/// <summary>路径平滑的最终发布状态；只有 Complete、PartialImprovement、NotNeeded 和 Unchanged 可携带完整路径。</summary>
public enum PathSmoothingStatus
{
Success,
FallbackToCoarsePath,
InvalidInput,
Infeasible,
Failed,
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PiecewiseQuinticOptions.cs
-11
View File
@@ -1,11 +0,0 @@
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
/// <summary>分段五次 Hermite 平滑参数。</summary>
public sealed class PiecewiseQuinticOptions
{
/// <summary>相邻内部结点的目标距离，单位 m。</summary>
public double KnotSpacingMeters { get; set; } = 0.50d;
/// <summary>允许创建内部结点的最小间距，单位 m。</summary>
public double MinimumKnotSpacingMeters { get; set; } = 0.10d;
}
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathPointSource.cs
+6 -3
View File
@@ -1,11 +1,14 @@
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
/// <summary>平滑路径点的来源。</summary>
/// <summary>平滑路径点的来源；用于区分保留锚点、常规重采样、换向锚点和 Local G2 过渡几何。</summary>
public enum SmoothedPathPointSource
{
/// <summary>直接保留的原始路径锚点。</summary>
Anchor,
/// <summary>在同一方向段内按弧长插值得到的常规采样点。</summary>
Interpolated,
/// <summary>精确保留的换向边界点；不能与相邻方向段按坐标合并。</summary>
GearSwitch,
CoarsePathFallback,
LocalG2Transition,
/// <summary>由 Local G2 五次过渡曲线生成的候选或发布点。</summary>
LocalG2Transition = 4,
}
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothingMethod.cs
+1 -4
View File
@@ -1,10 +1,7 @@
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
/// <summary>支持的粗路径平滑方法。</summary>
/// <summary>本模块可以发布的路径平滑方法枚举；当前仅支持 Local G2 五次过渡。</summary>
public enum SmoothingMethod
{
CubicBSpline,
LocalCubicBezier,
PiecewiseQuintic,
LocalG2Quintic,
}