feat: detect local curvature transition regions
This commit is contained in:
梁薄云
@@ -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>());
|
||||
}
|
||||
Reference in New Issue
Block a user