98 lines
4.7 KiB
C#
98 lines
4.7 KiB
C#
using System.Collections.Generic;
|
|
using System.Collections.ObjectModel;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.Mapping;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
|
|
|
/// <summary>路径平滑所需的原始粗路径、复核上下文和配置。</summary>
|
|
public sealed class PathSmoothingRequest
|
|
{
|
|
private readonly VehicleParameters _vehicle;
|
|
private readonly PathSmoothingConfiguration _configuration;
|
|
|
|
/// <summary>创建路径平滑请求，并复制粗路径和方向分段集合。</summary>
|
|
public PathSmoothingRequest(
|
|
IReadOnlyList<CoarsePathPoint> coarsePath,
|
|
IReadOnlyList<PathSegment> segments,
|
|
PlanningGridMap map,
|
|
VehicleParameters vehicle,
|
|
PathSmoothingConfiguration configuration)
|
|
{
|
|
CoarsePath = CopyReadOnly(coarsePath);
|
|
Segments = CopyReadOnly(segments);
|
|
Map = map;
|
|
_vehicle = CopyVehicle(vehicle);
|
|
_configuration = CopyConfiguration(configuration);
|
|
}
|
|
|
|
/// <summary>原始粗路径的不可变快照。</summary>
|
|
public IReadOnlyList<CoarsePathPoint> CoarsePath { get; }
|
|
|
|
/// <summary>原始粗路径方向分段的不可变快照。</summary>
|
|
public IReadOnlyList<PathSegment> Segments { get; }
|
|
|
|
/// <summary>用于平滑后完整车体复核的规划栅格地图。</summary>
|
|
public PlanningGridMap Map { get; }
|
|
|
|
/// <summary>车辆几何与最大曲率约束的不可变快照副本。</summary>
|
|
public VehicleParameters Vehicle => CopyVehicle(_vehicle);
|
|
|
|
/// <summary>本次平滑配置的不可变快照副本。</summary>
|
|
public PathSmoothingConfiguration Configuration => CopyConfiguration(_configuration);
|
|
|
|
private static IReadOnlyList<T> CopyReadOnly<T>(IReadOnlyList<T> source)
|
|
{
|
|
var copy = new List<T>(source == null ? 0 : source.Count);
|
|
if (source != null)
|
|
{
|
|
for (int index = 0; index < source.Count; index++)
|
|
copy.Add(source[index]);
|
|
}
|
|
return new ReadOnlyCollection<T>(copy);
|
|
}
|
|
|
|
private static VehicleParameters CopyVehicle(VehicleParameters source)
|
|
{
|
|
if (source == null) return null;
|
|
return new VehicleParameters
|
|
{
|
|
LengthMeters = source.LengthMeters,
|
|
WidthMeters = source.WidthMeters,
|
|
SafetyMarginMeters = source.SafetyMarginMeters,
|
|
MaximumCurvaturePerMeter = source.MaximumCurvaturePerMeter,
|
|
MinimumTurningRadiusMeters = source.MinimumTurningRadiusMeters,
|
|
};
|
|
}
|
|
|
|
private static PathSmoothingConfiguration CopyConfiguration(PathSmoothingConfiguration source)
|
|
{
|
|
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,
|
|
};
|
|
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;
|
|
copy.LocalG2Quintic.MaximumDeviationMeters = source.LocalG2Quintic.MaximumDeviationMeters;
|
|
copy.LocalG2Quintic.AbsoluteCurvatureJumpFloorPerMeter = source.LocalG2Quintic.AbsoluteCurvatureJumpFloorPerMeter;
|
|
copy.LocalG2Quintic.CurvatureJumpRatioOfMaximum = source.LocalG2Quintic.CurvatureJumpRatioOfMaximum;
|
|
copy.LocalG2Quintic.MinimumPeakGradientImprovementRatio = source.LocalG2Quintic.MinimumPeakGradientImprovementRatio;
|
|
copy.LocalG2Quintic.MaximumVariationCostRegressionRatio = source.LocalG2Quintic.MaximumVariationCostRegressionRatio;
|
|
copy.LocalG2Quintic.MaximumCandidatesPerRegion = source.LocalG2Quintic.MaximumCandidatesPerRegion;
|
|
return copy;
|
|
}
|
|
}
|