feat: detect local curvature transition regions
This commit is contained in:
梁薄云
@@ -0,0 +1,51 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
|
||||
/// <summary>同一方向原语边界两侧车辆曲率的离散跳变。</summary>
|
||||
internal sealed class CurvatureTransition
|
||||
{
|
||||
internal CurvatureTransition(
|
||||
int segmentIndex,
|
||||
int leftCoarsePathIndex,
|
||||
int rightCoarsePathIndex,
|
||||
double localArcLengthMeters,
|
||||
double x,
|
||||
double y,
|
||||
double vehicleHeadingRadians,
|
||||
double leftVehicleCurvaturePerMeter,
|
||||
double rightVehicleCurvaturePerMeter)
|
||||
{
|
||||
if (segmentIndex < 0 || leftCoarsePathIndex < 0 || rightCoarsePathIndex != leftCoarsePathIndex + 1)
|
||||
throw new ArgumentOutOfRangeException(nameof(leftCoarsePathIndex));
|
||||
if (!NumericGuard.IsFinite(localArcLengthMeters) || localArcLengthMeters < 0d ||
|
||||
!NumericGuard.IsFinite(x) || !NumericGuard.IsFinite(y) || !NumericGuard.IsFinite(vehicleHeadingRadians) ||
|
||||
!NumericGuard.IsFinite(leftVehicleCurvaturePerMeter) || !NumericGuard.IsFinite(rightVehicleCurvaturePerMeter))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(localArcLengthMeters));
|
||||
}
|
||||
|
||||
SegmentIndex = segmentIndex;
|
||||
LeftCoarsePathIndex = leftCoarsePathIndex;
|
||||
RightCoarsePathIndex = rightCoarsePathIndex;
|
||||
LocalArcLengthMeters = localArcLengthMeters;
|
||||
X = x;
|
||||
Y = y;
|
||||
VehicleHeadingRadians = vehicleHeadingRadians;
|
||||
LeftVehicleCurvaturePerMeter = leftVehicleCurvaturePerMeter;
|
||||
RightVehicleCurvaturePerMeter = rightVehicleCurvaturePerMeter;
|
||||
CurvatureJumpPerMeter = Math.Abs(rightVehicleCurvaturePerMeter - leftVehicleCurvaturePerMeter);
|
||||
}
|
||||
|
||||
internal int SegmentIndex { get; }
|
||||
internal int LeftCoarsePathIndex { get; }
|
||||
internal int RightCoarsePathIndex { get; }
|
||||
internal double LocalArcLengthMeters { get; }
|
||||
internal double X { get; }
|
||||
internal double Y { get; }
|
||||
internal double VehicleHeadingRadians { get; }
|
||||
internal double LeftVehicleCurvaturePerMeter { get; }
|
||||
internal double RightVehicleCurvaturePerMeter { get; }
|
||||
internal double CurvatureJumpPerMeter { get; }
|
||||
}
|
||||
@@ -0,0 +1,255 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
|
||||
/// <summary>从原始粗路径的同向相邻点中识别恒曲率原语边界。</summary>
|
||||
internal sealed class CurvatureTransitionDetector
|
||||
{
|
||||
internal bool TryDetect(
|
||||
PathSmoothingRequest request,
|
||||
double maximumVehicleCurvaturePerMeter,
|
||||
LocalG2OptionsSnapshot options,
|
||||
out IReadOnlyList<CurvatureTransition> transitions,
|
||||
out string reason)
|
||||
{
|
||||
transitions = Empty<CurvatureTransition>();
|
||||
reason = string.Empty;
|
||||
if (request == null || request.CoarsePath == null || request.Segments == null || options == null ||
|
||||
!NumericGuard.IsPositiveFinite(maximumVehicleCurvaturePerMeter))
|
||||
{
|
||||
reason = "局部 G2 曲率事件检测输入无效。";
|
||||
return false;
|
||||
}
|
||||
if (!ValidatePath(request.CoarsePath, out reason) || !ValidateSegments(request.CoarsePath, request.Segments, out reason))
|
||||
return false;
|
||||
|
||||
double threshold = Math.Max(
|
||||
options.AbsoluteCurvatureJumpFloorPerMeter,
|
||||
options.CurvatureJumpRatioOfMaximum * maximumVehicleCurvaturePerMeter);
|
||||
if (!NumericGuard.IsPositiveFinite(threshold))
|
||||
{
|
||||
reason = "局部 G2 曲率事件阈值无效。";
|
||||
return false;
|
||||
}
|
||||
|
||||
var detected = new List<CurvatureTransition>();
|
||||
for (int segmentPosition = 0; segmentPosition < request.Segments.Count; segmentPosition++)
|
||||
{
|
||||
PathSegment segment = request.Segments[segmentPosition];
|
||||
double segmentStartArc = request.CoarsePath[segment.StartIndex].ArcLength;
|
||||
for (int leftIndex = segment.StartIndex; leftIndex < segment.EndIndex; leftIndex++)
|
||||
{
|
||||
CoarsePathPoint left = request.CoarsePath[leftIndex];
|
||||
CoarsePathPoint right = request.CoarsePath[leftIndex + 1];
|
||||
double delta = right.VehicleCurvature - left.VehicleCurvature;
|
||||
if (Math.Abs(delta) >= threshold)
|
||||
{
|
||||
detected.Add(new CurvatureTransition(
|
||||
segment.SegmentIndex,
|
||||
leftIndex,
|
||||
leftIndex + 1,
|
||||
left.ArcLength - segmentStartArc,
|
||||
left.X,
|
||||
left.Y,
|
||||
left.Heading,
|
||||
left.VehicleCurvature,
|
||||
right.VehicleCurvature));
|
||||
}
|
||||
}
|
||||
}
|
||||
transitions = new ReadOnlyCollection<CurvatureTransition>(detected);
|
||||
return true;
|
||||
}
|
||||
|
||||
/// <summary>反射脚本使用的确定性检测与窗口规划接缝。</summary>
|
||||
public static class TestHooks
|
||||
{
|
||||
public static DetectionTestSnapshot Execute(string scenario)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(scenario)) throw new ArgumentException("A scenario is required.", nameof(scenario));
|
||||
switch (scenario)
|
||||
{
|
||||
case "SingleTransition": return DetectSingleTransition();
|
||||
case "GearSwitch": return DetectGearSwitch();
|
||||
case "Noise": return DetectNoise();
|
||||
case "Overlap": return PlanOverlap();
|
||||
case "NearStart": return PlanNearStart();
|
||||
default: throw new ArgumentOutOfRangeException(nameof(scenario));
|
||||
}
|
||||
}
|
||||
|
||||
public sealed class DetectionTestSnapshot
|
||||
{
|
||||
internal DetectionTestSnapshot(int transitionCount, double maximumJump, int regionCount,
|
||||
int transitionCountInFirstRegion, double startArcLength, double leftWindowLength, double rightWindowLength)
|
||||
{
|
||||
TransitionCount = transitionCount;
|
||||
MaximumJump = maximumJump;
|
||||
RegionCount = regionCount;
|
||||
TransitionCountInFirstRegion = transitionCountInFirstRegion;
|
||||
StartArcLength = startArcLength;
|
||||
LeftWindowLength = leftWindowLength;
|
||||
RightWindowLength = rightWindowLength;
|
||||
}
|
||||
|
||||
public int TransitionCount { get; }
|
||||
public double MaximumJump { get; }
|
||||
public int RegionCount { get; }
|
||||
public int TransitionCountInFirstRegion { get; }
|
||||
public double StartArcLength { get; }
|
||||
public double LeftWindowLength { get; }
|
||||
public double RightWindowLength { get; }
|
||||
}
|
||||
|
||||
private static DetectionTestSnapshot DetectSingleTransition()
|
||||
{
|
||||
IReadOnlyList<CurvatureTransition> transitions = Detect(CreateRequest(
|
||||
new[] { Point(0d, 0d), Point(0.1d, 0.4167d), Point(0.2d, 0.4167d) },
|
||||
new[] { new PathSegment(0, TravelDirection.Forward, 0, 2, false, false) }));
|
||||
return Snapshot(transitions);
|
||||
}
|
||||
|
||||
private static DetectionTestSnapshot DetectGearSwitch()
|
||||
{
|
||||
IReadOnlyList<CurvatureTransition> transitions = Detect(CreateRequest(
|
||||
new[]
|
||||
{
|
||||
Point(0d, 0d, TravelDirection.Forward), Point(0.2d, 0d, TravelDirection.Forward),
|
||||
Point(0.2d, 0.4167d, TravelDirection.Reverse, true), Point(0.4d, 0.4167d, TravelDirection.Reverse),
|
||||
},
|
||||
new[]
|
||||
{
|
||||
new PathSegment(0, TravelDirection.Forward, 0, 1, false, true),
|
||||
new PathSegment(1, TravelDirection.Reverse, 2, 3, true, false),
|
||||
}));
|
||||
return Snapshot(transitions);
|
||||
}
|
||||
|
||||
private static DetectionTestSnapshot DetectNoise()
|
||||
{
|
||||
IReadOnlyList<CurvatureTransition> transitions = Detect(CreateRequest(
|
||||
new[] { Point(0d, 0d), Point(0.1d, 0.01d), Point(0.2d, 0.01d) },
|
||||
new[] { new PathSegment(0, TravelDirection.Forward, 0, 2, false, false) }));
|
||||
return Snapshot(transitions);
|
||||
}
|
||||
|
||||
private static DetectionTestSnapshot PlanOverlap()
|
||||
{
|
||||
LocalG2OptionsSnapshot options = CreateOptions();
|
||||
var transitions = new[]
|
||||
{
|
||||
new CurvatureTransition(0, 1, 2, 0.4d, 0.4d, 0d, 0d, 0d, 0.5d),
|
||||
new CurvatureTransition(0, 2, 3, 0.7d, 0.7d, 0d, 0d, 0.5d, 0d),
|
||||
};
|
||||
var planner = new LocalG2WindowPlanner();
|
||||
if (!planner.TryPlan(CreatePreparedPath(1.4d), transitions, options, out IReadOnlyList<LocalG2SmoothingRegion> regions, out string reason))
|
||||
throw new InvalidOperationException(reason);
|
||||
return new DetectionTestSnapshot(2, 0.5d, regions.Count, regions[0].Transitions.Count, 0d, 0d, 0d);
|
||||
}
|
||||
|
||||
private static DetectionTestSnapshot PlanNearStart()
|
||||
{
|
||||
LocalG2OptionsSnapshot options = CreateOptions();
|
||||
var transitions = new[] { new CurvatureTransition(0, 0, 1, 0.1d, 0.1d, 0d, 0d, 0d, 0.5d) };
|
||||
var planner = new LocalG2WindowPlanner();
|
||||
if (!planner.TryPlan(CreatePreparedPath(1d), transitions, options, out IReadOnlyList<LocalG2SmoothingRegion> regions, out string reason))
|
||||
throw new InvalidOperationException(reason);
|
||||
LocalG2WindowVariant first = regions[0].WindowVariants[0];
|
||||
return new DetectionTestSnapshot(1, 0.5d, regions.Count, 1,
|
||||
first.StartArcLengthMeters, first.LeftWindowLengthMeters, first.RightWindowLengthMeters);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<CurvatureTransition> Detect(PathSmoothingRequest request)
|
||||
{
|
||||
var detector = new CurvatureTransitionDetector();
|
||||
if (!detector.TryDetect(request, 0.8333d, CreateOptions(), out IReadOnlyList<CurvatureTransition> transitions, out string reason))
|
||||
throw new InvalidOperationException(reason);
|
||||
return transitions;
|
||||
}
|
||||
|
||||
private static DetectionTestSnapshot Snapshot(IReadOnlyList<CurvatureTransition> transitions)
|
||||
{
|
||||
double maximum = 0d;
|
||||
for (int index = 0; index < transitions.Count; index++) maximum = Math.Max(maximum, transitions[index].CurvatureJumpPerMeter);
|
||||
return new DetectionTestSnapshot(transitions.Count, maximum, 0, 0, 0d, 0d, 0d);
|
||||
}
|
||||
|
||||
private static PathSmoothingRequest CreateRequest(IReadOnlyList<CoarsePathPoint> points, IReadOnlyList<PathSegment> segments)
|
||||
{
|
||||
return new PathSmoothingRequest(points, segments, null, null, new PathSmoothingConfiguration());
|
||||
}
|
||||
|
||||
private static CoarsePathPoint Point(double arcLength, double curvature, TravelDirection direction = TravelDirection.Forward, bool gearSwitch = false)
|
||||
{
|
||||
return new CoarsePathPoint(arcLength, 0d, 0d, 0d, arcLength, direction, curvature, 1d, gearSwitch,
|
||||
CoarsePathPointSource.MotionPrimitive);
|
||||
}
|
||||
|
||||
private static LocalG2OptionsSnapshot CreateOptions() => new LocalG2OptionsSnapshot(new PathSmoothingConfiguration());
|
||||
|
||||
private static Processing.PreparedPath CreatePreparedPath(double length)
|
||||
{
|
||||
var points = new[]
|
||||
{
|
||||
new Processing.SmoothingPoint2D(0d, 0d, 0d, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
|
||||
new Processing.SmoothingPoint2D(length, 0d, length, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
|
||||
};
|
||||
var segment = new Processing.PreparedDirectionSegment(0, TravelDirection.Forward, points, false, false);
|
||||
return new Processing.PreparedPath(new[] { segment });
|
||||
}
|
||||
}
|
||||
|
||||
private static bool ValidatePath(IReadOnlyList<CoarsePathPoint> path, out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
if (path.Count == 0)
|
||||
{
|
||||
reason = "局部 G2 曲率事件检测需要粗路径点。";
|
||||
return false;
|
||||
}
|
||||
double previousArc = -1d;
|
||||
for (int index = 0; index < path.Count; index++)
|
||||
{
|
||||
CoarsePathPoint point = path[index];
|
||||
if (point == null || !NumericGuard.IsFinite(point.X) || !NumericGuard.IsFinite(point.Y) ||
|
||||
!NumericGuard.IsFinite(point.Heading) || !NumericGuard.IsFinite(point.ArcLength) || point.ArcLength < 0d ||
|
||||
!NumericGuard.IsFinite(point.VehicleCurvature) || point.ArcLength < previousArc)
|
||||
{
|
||||
reason = "局部 G2 曲率事件检测要求有限且非递减的粗路径。";
|
||||
return false;
|
||||
}
|
||||
previousArc = point.ArcLength;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static bool ValidateSegments(IReadOnlyList<CoarsePathPoint> path, IReadOnlyList<PathSegment> segments, out string reason)
|
||||
{
|
||||
reason = string.Empty;
|
||||
for (int position = 0; position < segments.Count; position++)
|
||||
{
|
||||
PathSegment segment = segments[position];
|
||||
if (segment == null || segment.SegmentIndex != position || segment.StartIndex < 0 ||
|
||||
segment.EndIndex < segment.StartIndex || segment.EndIndex >= path.Count)
|
||||
{
|
||||
reason = "局部 G2 曲率事件检测的方向分段无效。";
|
||||
return false;
|
||||
}
|
||||
for (int index = segment.StartIndex; index <= segment.EndIndex; index++)
|
||||
{
|
||||
if (path[index].Direction != segment.Direction)
|
||||
{
|
||||
reason = "局部 G2 曲率事件检测的方向分段包含换向点。";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static IReadOnlyList<T> Empty<T>() => new ReadOnlyCollection<T>(new List<T>());
|
||||
}
|
||||
@@ -0,0 +1,57 @@
|
||||
using System;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
|
||||
/// <summary>局部 G2 预平滑一次运行使用的已校验不可变选项。</summary>
|
||||
internal sealed class LocalG2OptionsSnapshot
|
||||
{
|
||||
internal LocalG2OptionsSnapshot(PathSmoothingConfiguration configuration)
|
||||
{
|
||||
if (configuration == null) throw new ArgumentNullException(nameof(configuration));
|
||||
LocalG2QuinticOptions source = configuration.LocalG2Quintic;
|
||||
if (source == null) throw new ArgumentOutOfRangeException(nameof(configuration));
|
||||
|
||||
MinimumWindowLengthMeters = source.MinimumWindowLengthMeters;
|
||||
PreferredWindowLengthMeters = source.PreferredWindowLengthMeters;
|
||||
MaximumWindowLengthMeters = source.MaximumWindowLengthMeters;
|
||||
MaximumDeviationMeters = source.MaximumDeviationMeters;
|
||||
AbsoluteCurvatureJumpFloorPerMeter = source.AbsoluteCurvatureJumpFloorPerMeter;
|
||||
CurvatureJumpRatioOfMaximum = source.CurvatureJumpRatioOfMaximum;
|
||||
MinimumPeakGradientImprovementRatio = source.MinimumPeakGradientImprovementRatio;
|
||||
MaximumVariationCostRegressionRatio = source.MaximumVariationCostRegressionRatio;
|
||||
MaximumCandidatesPerRegion = source.MaximumCandidatesPerRegion;
|
||||
|
||||
if (!NumericGuard.IsPositiveFinite(MinimumWindowLengthMeters))
|
||||
throw new ArgumentOutOfRangeException(nameof(MinimumWindowLengthMeters));
|
||||
if (!NumericGuard.IsFinite(PreferredWindowLengthMeters) || PreferredWindowLengthMeters < MinimumWindowLengthMeters)
|
||||
throw new ArgumentOutOfRangeException(nameof(PreferredWindowLengthMeters));
|
||||
if (!NumericGuard.IsFinite(MaximumWindowLengthMeters) || MaximumWindowLengthMeters < PreferredWindowLengthMeters)
|
||||
throw new ArgumentOutOfRangeException(nameof(MaximumWindowLengthMeters));
|
||||
if (!NumericGuard.IsPositiveFinite(MaximumDeviationMeters))
|
||||
throw new ArgumentOutOfRangeException(nameof(MaximumDeviationMeters));
|
||||
if (!NumericGuard.IsPositiveFinite(AbsoluteCurvatureJumpFloorPerMeter))
|
||||
throw new ArgumentOutOfRangeException(nameof(AbsoluteCurvatureJumpFloorPerMeter));
|
||||
if (!NumericGuard.IsFinite(CurvatureJumpRatioOfMaximum) || CurvatureJumpRatioOfMaximum <= 0d || CurvatureJumpRatioOfMaximum > 1d)
|
||||
throw new ArgumentOutOfRangeException(nameof(CurvatureJumpRatioOfMaximum));
|
||||
if (!NumericGuard.IsFinite(MinimumPeakGradientImprovementRatio) ||
|
||||
MinimumPeakGradientImprovementRatio <= 0d || MinimumPeakGradientImprovementRatio >= 1d)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(MinimumPeakGradientImprovementRatio));
|
||||
}
|
||||
if (!NumericGuard.IsFinite(MaximumVariationCostRegressionRatio) || MaximumVariationCostRegressionRatio < 0d)
|
||||
throw new ArgumentOutOfRangeException(nameof(MaximumVariationCostRegressionRatio));
|
||||
if (MaximumCandidatesPerRegion < 1)
|
||||
throw new ArgumentOutOfRangeException(nameof(MaximumCandidatesPerRegion));
|
||||
}
|
||||
|
||||
internal double MinimumWindowLengthMeters { get; }
|
||||
internal double PreferredWindowLengthMeters { get; }
|
||||
internal double MaximumWindowLengthMeters { get; }
|
||||
internal double MaximumDeviationMeters { get; }
|
||||
internal double AbsoluteCurvatureJumpFloorPerMeter { get; }
|
||||
internal double CurvatureJumpRatioOfMaximum { get; }
|
||||
internal double MinimumPeakGradientImprovementRatio { get; }
|
||||
internal double MaximumVariationCostRegressionRatio { get; }
|
||||
internal int MaximumCandidatesPerRegion { get; }
|
||||
}
|
||||
@@ -0,0 +1,67 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
|
||||
/// <summary>一个局部 G2 候选可替换的弧长窗口。</summary>
|
||||
internal sealed class LocalG2WindowVariant
|
||||
{
|
||||
internal LocalG2WindowVariant(int candidateIndex, double startArcLengthMeters, double endArcLengthMeters,
|
||||
double leftWindowLengthMeters, double rightWindowLengthMeters)
|
||||
{
|
||||
if (candidateIndex < 0 || startArcLengthMeters < 0d || endArcLengthMeters < startArcLengthMeters ||
|
||||
leftWindowLengthMeters < 0d || rightWindowLengthMeters < 0d)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(candidateIndex));
|
||||
}
|
||||
CandidateIndex = candidateIndex;
|
||||
StartArcLengthMeters = startArcLengthMeters;
|
||||
EndArcLengthMeters = endArcLengthMeters;
|
||||
LeftWindowLengthMeters = leftWindowLengthMeters;
|
||||
RightWindowLengthMeters = rightWindowLengthMeters;
|
||||
}
|
||||
|
||||
internal int CandidateIndex { get; }
|
||||
internal double StartArcLengthMeters { get; }
|
||||
internal double EndArcLengthMeters { get; }
|
||||
internal double LeftWindowLengthMeters { get; }
|
||||
internal double RightWindowLengthMeters { get; }
|
||||
}
|
||||
|
||||
/// <summary>因最大合法窗口相交而合并的一组曲率事件。</summary>
|
||||
internal sealed class LocalG2SmoothingRegion
|
||||
{
|
||||
internal LocalG2SmoothingRegion(
|
||||
int segmentIndex,
|
||||
IReadOnlyList<CurvatureTransition> transitions,
|
||||
double maximumStartArcLengthMeters,
|
||||
double maximumEndArcLengthMeters,
|
||||
IReadOnlyList<LocalG2WindowVariant> windowVariants)
|
||||
{
|
||||
if (segmentIndex < 0 || transitions == null || transitions.Count == 0 ||
|
||||
maximumStartArcLengthMeters < 0d || maximumEndArcLengthMeters < maximumStartArcLengthMeters ||
|
||||
windowVariants == null)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(nameof(transitions));
|
||||
}
|
||||
SegmentIndex = segmentIndex;
|
||||
Transitions = Copy(transitions);
|
||||
MaximumStartArcLengthMeters = maximumStartArcLengthMeters;
|
||||
MaximumEndArcLengthMeters = maximumEndArcLengthMeters;
|
||||
WindowVariants = Copy(windowVariants);
|
||||
}
|
||||
|
||||
internal int SegmentIndex { get; }
|
||||
internal IReadOnlyList<CurvatureTransition> Transitions { get; }
|
||||
internal double MaximumStartArcLengthMeters { get; }
|
||||
internal double MaximumEndArcLengthMeters { get; }
|
||||
internal IReadOnlyList<LocalG2WindowVariant> WindowVariants { get; }
|
||||
|
||||
private static IReadOnlyList<T> Copy<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);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,212 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
|
||||
using MultiWheelC.TrajectoryPlanning.Utils;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;
|
||||
|
||||
/// <summary>按硬方向边界生成并合并局部 G2 曲率事件的候选窗口。</summary>
|
||||
internal sealed class LocalG2WindowPlanner
|
||||
{
|
||||
private const double MergeToleranceMeters = 1e-9d;
|
||||
|
||||
internal bool TryPlan(
|
||||
PreparedPath originalPath,
|
||||
IReadOnlyList<CurvatureTransition> transitions,
|
||||
LocalG2OptionsSnapshot options,
|
||||
out IReadOnlyList<LocalG2SmoothingRegion> regions,
|
||||
out string reason)
|
||||
{
|
||||
regions = Empty<LocalG2SmoothingRegion>();
|
||||
reason = string.Empty;
|
||||
if (originalPath == null || transitions == null || options == null)
|
||||
{
|
||||
reason = "局部 G2 窗口规划输入无效。";
|
||||
return false;
|
||||
}
|
||||
if (!TryGetSegmentLengths(originalPath, out Dictionary<int, double> segmentLengths, out reason)) return false;
|
||||
|
||||
var ordered = new List<CurvatureTransition>(transitions.Count);
|
||||
for (int index = 0; index < transitions.Count; index++)
|
||||
{
|
||||
CurvatureTransition transition = transitions[index];
|
||||
if (transition == null || !segmentLengths.TryGetValue(transition.SegmentIndex, out double length) ||
|
||||
!NumericGuard.IsFinite(transition.LocalArcLengthMeters) || transition.LocalArcLengthMeters < 0d ||
|
||||
transition.LocalArcLengthMeters > length + MergeToleranceMeters)
|
||||
{
|
||||
reason = "局部 G2 曲率事件不属于有效方向分段。";
|
||||
return false;
|
||||
}
|
||||
ordered.Add(transition);
|
||||
}
|
||||
ordered.Sort(CompareTransitions);
|
||||
|
||||
var planned = new List<LocalG2SmoothingRegion>();
|
||||
int cursor = 0;
|
||||
while (cursor < ordered.Count)
|
||||
{
|
||||
CurvatureTransition first = ordered[cursor];
|
||||
double segmentLength = segmentLengths[first.SegmentIndex];
|
||||
WindowRange merged = MaximumLegalRange(first.LocalArcLengthMeters, segmentLength, options.MaximumWindowLengthMeters);
|
||||
var group = new List<CurvatureTransition> { first };
|
||||
cursor++;
|
||||
|
||||
while (cursor < ordered.Count && ordered[cursor].SegmentIndex == first.SegmentIndex)
|
||||
{
|
||||
CurvatureTransition next = ordered[cursor];
|
||||
WindowRange nextRange = MaximumLegalRange(next.LocalArcLengthMeters, segmentLength, options.MaximumWindowLengthMeters);
|
||||
if (nextRange.StartArcLengthMeters > merged.EndArcLengthMeters + MergeToleranceMeters) break;
|
||||
group.Add(next);
|
||||
merged = new WindowRange(
|
||||
Math.Min(merged.StartArcLengthMeters, nextRange.StartArcLengthMeters),
|
||||
Math.Max(merged.EndArcLengthMeters, nextRange.EndArcLengthMeters));
|
||||
cursor++;
|
||||
}
|
||||
|
||||
planned.Add(new LocalG2SmoothingRegion(
|
||||
first.SegmentIndex,
|
||||
group,
|
||||
merged.StartArcLengthMeters,
|
||||
merged.EndArcLengthMeters,
|
||||
BuildVariants(group, segmentLength, options)));
|
||||
}
|
||||
regions = new ReadOnlyCollection<LocalG2SmoothingRegion>(planned);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static IReadOnlyList<LocalG2WindowVariant> BuildVariants(
|
||||
IReadOnlyList<CurvatureTransition> transitions,
|
||||
double segmentLength,
|
||||
LocalG2OptionsSnapshot options)
|
||||
{
|
||||
var variants = new List<LocalG2WindowVariant>();
|
||||
double firstEvent = transitions[0].LocalArcLengthMeters;
|
||||
double lastEvent = transitions[transitions.Count - 1].LocalArcLengthMeters;
|
||||
double anchor = (firstEvent + lastEvent) / 2d;
|
||||
foreach (double target in BuildTargets(options, segmentLength))
|
||||
{
|
||||
if (variants.Count >= options.MaximumCandidatesPerRegion) break;
|
||||
AddIfLegal(variants, target, 0.5d, anchor, firstEvent, lastEvent, segmentLength, true, options.MaximumCandidatesPerRegion);
|
||||
AddIfLegal(variants, target, 0.4d, anchor, firstEvent, lastEvent, segmentLength, false, options.MaximumCandidatesPerRegion);
|
||||
AddIfLegal(variants, target, 0.6d, anchor, firstEvent, lastEvent, segmentLength, false, options.MaximumCandidatesPerRegion);
|
||||
}
|
||||
return new ReadOnlyCollection<LocalG2WindowVariant>(variants);
|
||||
}
|
||||
|
||||
private static void AddIfLegal(List<LocalG2WindowVariant> variants, double target, double leftRatio,
|
||||
double anchor, double firstEvent, double lastEvent, double segmentLength, bool permitBoundaryShift, int maximumCount)
|
||||
{
|
||||
if (variants.Count >= maximumCount) return;
|
||||
double left = target * leftRatio;
|
||||
double right = target - left;
|
||||
double availableLeft = anchor;
|
||||
double availableRight = segmentLength - anchor;
|
||||
if (permitBoundaryShift)
|
||||
{
|
||||
left = Math.Min(left, availableLeft);
|
||||
right = Math.Min(right, availableRight);
|
||||
double missing = target - left - right;
|
||||
double addRight = Math.Min(missing, availableRight - right);
|
||||
right += addRight;
|
||||
left += Math.Min(missing - addRight, availableLeft - left);
|
||||
}
|
||||
else if (left > availableLeft + MergeToleranceMeters || right > availableRight + MergeToleranceMeters)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
double start = anchor - left;
|
||||
double end = anchor + right;
|
||||
if (start > firstEvent + MergeToleranceMeters || end + MergeToleranceMeters < lastEvent || end - start + MergeToleranceMeters < target)
|
||||
return;
|
||||
variants.Add(new LocalG2WindowVariant(variants.Count, start, end, left, right));
|
||||
}
|
||||
|
||||
private static IReadOnlyList<double> BuildTargets(LocalG2OptionsSnapshot options, double segmentLength)
|
||||
{
|
||||
double[] requested =
|
||||
{
|
||||
options.PreferredWindowLengthMeters,
|
||||
0.75d * options.PreferredWindowLengthMeters,
|
||||
1.25d * options.PreferredWindowLengthMeters,
|
||||
options.MinimumWindowLengthMeters,
|
||||
options.MaximumWindowLengthMeters,
|
||||
};
|
||||
var targets = new List<double>(requested.Length);
|
||||
for (int index = 0; index < requested.Length; index++)
|
||||
{
|
||||
double target = Math.Min(segmentLength,
|
||||
Math.Max(options.MinimumWindowLengthMeters, Math.Min(options.MaximumWindowLengthMeters, requested[index])));
|
||||
bool duplicate = false;
|
||||
for (int prior = 0; prior < targets.Count; prior++)
|
||||
{
|
||||
if (Math.Abs(targets[prior] - target) <= MergeToleranceMeters)
|
||||
{
|
||||
duplicate = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!duplicate) targets.Add(target);
|
||||
}
|
||||
return targets;
|
||||
}
|
||||
|
||||
private static WindowRange MaximumLegalRange(double eventArcLength, double segmentLength, double maximumWindowLength)
|
||||
{
|
||||
double target = Math.Min(maximumWindowLength, segmentLength);
|
||||
return new WindowRange(
|
||||
Math.Max(0d, eventArcLength - target),
|
||||
Math.Min(segmentLength, eventArcLength + target));
|
||||
}
|
||||
|
||||
private static bool TryGetSegmentLengths(PreparedPath originalPath, out Dictionary<int, double> lengths, out string reason)
|
||||
{
|
||||
lengths = new Dictionary<int, double>();
|
||||
reason = string.Empty;
|
||||
for (int position = 0; position < originalPath.Segments.Count; position++)
|
||||
{
|
||||
PreparedDirectionSegment segment = originalPath.Segments[position];
|
||||
if (segment == null || segment.SegmentIndex != position || segment.Points == null || segment.Points.Count == 0)
|
||||
{
|
||||
reason = "局部 G2 窗口规划的预处理方向分段无效。";
|
||||
return false;
|
||||
}
|
||||
double previousArc = -1d;
|
||||
for (int pointIndex = 0; pointIndex < segment.Points.Count; pointIndex++)
|
||||
{
|
||||
double arc = segment.Points[pointIndex].ArcLength;
|
||||
if (!NumericGuard.IsFinite(arc) || arc < 0d || arc < previousArc)
|
||||
{
|
||||
reason = "局部 G2 窗口规划要求分段弧长有限且非递减。";
|
||||
return false;
|
||||
}
|
||||
previousArc = arc;
|
||||
}
|
||||
lengths.Add(segment.SegmentIndex, previousArc);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private static int CompareTransitions(CurvatureTransition left, CurvatureTransition right)
|
||||
{
|
||||
int segment = left.SegmentIndex.CompareTo(right.SegmentIndex);
|
||||
if (segment != 0) return segment;
|
||||
int arc = left.LocalArcLengthMeters.CompareTo(right.LocalArcLengthMeters);
|
||||
return arc != 0 ? arc : left.LeftCoarsePathIndex.CompareTo(right.LeftCoarsePathIndex);
|
||||
}
|
||||
|
||||
private static IReadOnlyList<T> Empty<T>() => new ReadOnlyCollection<T>(new List<T>());
|
||||
|
||||
private readonly struct WindowRange
|
||||
{
|
||||
internal WindowRange(double startArcLengthMeters, double endArcLengthMeters)
|
||||
{
|
||||
StartArcLengthMeters = startArcLengthMeters;
|
||||
EndArcLengthMeters = endArcLengthMeters;
|
||||
}
|
||||
|
||||
internal double StartArcLengthMeters { get; }
|
||||
internal double EndArcLengthMeters { get; }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,58 @@
|
||||
param([string]$AssemblyPath = (Join-Path $PSScriptRoot '..\bin\Debug\netstandard2.0\ClumsyPilot.dll'))
|
||||
|
||||
$ErrorActionPreference = 'Stop'
|
||||
$assembly = [Reflection.Assembly]::LoadFrom((Resolve-Path $AssemblyPath))
|
||||
|
||||
function Assert-True($Actual, [string]$Message) {
|
||||
if (-not $Actual) { throw $Message }
|
||||
}
|
||||
|
||||
function Assert-Equal($Expected, $Actual, [string]$Message) {
|
||||
if ($Expected -ne $Actual) { throw "$Message Expected=$Expected Actual=$Actual" }
|
||||
}
|
||||
|
||||
function Assert-Near([double]$Expected, [double]$Actual, [double]$Tolerance, [string]$Message) {
|
||||
if ([Math]::Abs($Expected - $Actual) -gt $Tolerance) {
|
||||
throw "$Message Expected=$Expected Actual=$Actual Tolerance=$Tolerance"
|
||||
}
|
||||
}
|
||||
|
||||
function Get-RequiredType([string]$Name) {
|
||||
return $assembly.GetType($Name, $true)
|
||||
}
|
||||
|
||||
$detectorType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2.CurvatureTransitionDetector'
|
||||
$hooksType = $detectorType.GetNestedType('TestHooks', [Reflection.BindingFlags]'Public,NonPublic')
|
||||
Assert-True ($null -ne $hooksType) 'CurvatureTransitionDetector must expose its narrowly scoped nested TestHooks helper.'
|
||||
$executeMethod = $hooksType.GetMethod('Execute', [Reflection.BindingFlags]'Public,Static')
|
||||
Assert-True ($null -ne $executeMethod) 'TestHooks must expose deterministic scenario execution for reflection tests.'
|
||||
|
||||
function Invoke-Scenario([string]$Scenario) {
|
||||
return $executeMethod.Invoke($null, @($Scenario))
|
||||
}
|
||||
|
||||
# Same direction: 0 -> 0.4167 is detected once.
|
||||
$singleTransition = Invoke-Scenario 'SingleTransition'
|
||||
Assert-Equal 1 $singleTransition.TransitionCount 'One primitive curvature jump must be detected.'
|
||||
Assert-Near 0.4167 $singleTransition.MaximumJump 0.0001 'The jump magnitude must be retained.'
|
||||
|
||||
# Same pose at a forward/reverse boundary: no event crosses the stop.
|
||||
$gearSwitch = Invoke-Scenario 'GearSwitch'
|
||||
Assert-Equal 0 $gearSwitch.TransitionCount 'A stopped gear switch must not be a smoothing event.'
|
||||
|
||||
# A 0.01 1/m numerical change is below max(0.001, 5% of 0.8333).
|
||||
$noise = Invoke-Scenario 'Noise'
|
||||
Assert-Equal 0 $noise.TransitionCount 'Sub-threshold curvature noise must be ignored.'
|
||||
|
||||
# Two 0.50 m windows whose ranges overlap are merged.
|
||||
$overlap = Invoke-Scenario 'Overlap'
|
||||
Assert-Equal 1 $overlap.RegionCount 'Overlapping windows must form one joint region.'
|
||||
Assert-Equal 2 $overlap.TransitionCountInFirstRegion 'The merged region must retain both events.'
|
||||
|
||||
# Near a segment start, the window becomes asymmetric without crossing the hard boundary.
|
||||
$nearStart = Invoke-Scenario 'NearStart'
|
||||
Assert-Near 0.0 $nearStart.StartArcLength 0.000000001 'A start window must be clamped to the segment.'
|
||||
Assert-True ($nearStart.RightWindowLength -gt $nearStart.LeftWindowLength) `
|
||||
'Unavailable left length must be shifted to the right.'
|
||||
|
||||
Write-Output 'Path smoothing Local G2 detection checks passed.'
|
||||
Reference in New Issue
Block a user