256 lines
12 KiB
C#
256 lines
12 KiB
C#
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>());
|
|
}
|