ParkingRobot/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathSmoothingPreprocessor.cs

using System;
using System.Collections.Generic;
using MultiWheelC.TrajectoryPlanning.CoarsePath;
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;

/// <summary>校验粗路径契约、保护换向拓扑并产生统一间距的平滑输入。</summary>
public sealed class PathSmoothingPreprocessor
{
    private const double Tolerance = 1e-8d;
    private readonly ArcLengthResampler _resampler;

    /// <summary>创建使用默认确定性重采样器的预处理器。</summary>
    public PathSmoothingPreprocessor()
        : this(new ArcLengthResampler())
    {
    }

    /// <summary>创建使用指定重采样器的预处理器。</summary>
    public PathSmoothingPreprocessor(ArcLengthResampler resampler)
    {
        _resampler = resampler ?? throw new ArgumentNullException(nameof(resampler));
    }

    /// <summary>将一条粗路径请求校验、按方向拆分并按配置间距重采样。</summary>
    public bool TryPrepare(PathSmoothingRequest request, out PreparedPath preparedPath, out string reason)
    {
        preparedPath = null;
        reason = string.Empty;
        if (request == null || request.Map == null || request.Vehicle == null || request.Configuration == null ||
            request.CoarsePath == null || request.Segments == null || request.CoarsePath.Count == 0 || request.Segments.Count == 0)
        {
            reason = "平滑请求缺少粗路径、方向分段、地图、车辆或配置。";
            return false;
        }

        PathSmoothingConfiguration configuration = request.Configuration;
        if (!NumericGuard.IsPositiveFinite(configuration.OutputSpacingMeters))
        {
            reason = "平滑输出采样间距无效。";
            return false;
        }

        if (!ValidatePathPoints(request.CoarsePath, out reason) ||
            !ValidateSegments(request.CoarsePath, request.Segments, out reason))
        {
            return false;
        }

        var preparedSegments = new List<PreparedDirectionSegment>(request.Segments.Count);
        for (int segmentIndex = 0; segmentIndex < request.Segments.Count; segmentIndex++)
        {
            PathSegment sourceSegment = request.Segments[segmentIndex];
            double segmentStartArcLength = request.CoarsePath[sourceSegment.StartIndex].ArcLength;
            var segmentPoints = new List<SmoothingPoint2D>(sourceSegment.EndIndex - sourceSegment.StartIndex + 1);
            for (int pointIndex = sourceSegment.StartIndex; pointIndex <= sourceSegment.EndIndex; pointIndex++)
            {
                CoarsePathPoint point = request.CoarsePath[pointIndex];
                bool isGearSwitch = point.IsGearSwitchPoint;
                segmentPoints.Add(new SmoothingPoint2D(
                    point.X,
                    point.Y,
                    point.ArcLength - segmentStartArcLength,
                    point.Heading,
                    point.UnwrappedHeading,
                    point.BodyClearance,
                    isGearSwitch,
                    isGearSwitch ? SmoothedPathPointSource.GearSwitch : SmoothedPathPointSource.Anchor));
            }

            var unresampled = new PreparedDirectionSegment(
                sourceSegment.SegmentIndex,
                sourceSegment.Direction,
                segmentPoints,
                sourceSegment.StartsAtGearSwitch,
                sourceSegment.EndsAtGearSwitch);
            if (!_resampler.TryResample(unresampled, configuration.OutputSpacingMeters, out PreparedDirectionSegment resampled, out reason))
                return false;
            preparedSegments.Add(resampled);
        }

        preparedPath = new PreparedPath(preparedSegments);
        return true;
    }

    private static bool ValidatePathPoints(IReadOnlyList<CoarsePathPoint> path, out string reason)
    {
        reason = string.Empty;
        CoarsePathPoint first = path[0];
        if (!IsValidPoint(first) || first.IsGearSwitchPoint || Math.Abs(first.ArcLength) > Tolerance)
        {
            reason = "粗路径首点无效。";
            return false;
        }

        for (int index = 1; index < path.Count; index++)
        {
            CoarsePathPoint previous = path[index - 1];
            CoarsePathPoint current = path[index];
            if (!IsValidPoint(current) || current.ArcLength + Tolerance < previous.ArcLength)
            {
                reason = "粗路径包含非法数值或非递增弧长。";
                return false;
            }

            double expectedHeadingDelta = AngleMath.ShortestSignedDifference(previous.Heading, current.Heading);
            if (!NumericGuard.IsFinite(expectedHeadingDelta) ||
                Math.Abs((current.UnwrappedHeading - previous.UnwrappedHeading) - expectedHeadingDelta) > Tolerance)
            {
                reason = "粗路径展开航向不连续。";
                return false;
            }

            bool duplicatePoseAndArc = Math.Abs(current.X - previous.X) <= Tolerance &&
                Math.Abs(current.Y - previous.Y) <= Tolerance &&
                Math.Abs(current.ArcLength - previous.ArcLength) <= Tolerance &&
                Math.Abs(AngleMath.ShortestSignedDifference(previous.Heading, current.Heading)) <= Tolerance;
            if (duplicatePoseAndArc)
            {
                if (previous.Direction == current.Direction || !current.IsGearSwitchPoint)
                {
                    reason = "粗路径包含非法的重复点。";
                    return false;
                }
            }
            else if (current.IsGearSwitchPoint || current.ArcLength <= previous.ArcLength + Tolerance)
            {
                reason = "粗路径普通点必须有正弧长增量且不得标记为换向点。";
                return false;
            }
        }
        return true;
    }

    private static bool ValidateSegments(
        IReadOnlyList<CoarsePathPoint> path,
        IReadOnlyList<PathSegment> segments,
        out string reason)
    {
        reason = string.Empty;
        int expectedStartIndex = 0;
        for (int segmentIndex = 0; segmentIndex < segments.Count; segmentIndex++)
        {
            PathSegment segment = segments[segmentIndex];
            if (segment == null || segment.SegmentIndex != segmentIndex || segment.StartIndex != expectedStartIndex ||
                segment.StartIndex < 0 || segment.EndIndex < segment.StartIndex || segment.EndIndex >= path.Count ||
                segment.StartsAtGearSwitch != path[segment.StartIndex].IsGearSwitchPoint)
            {
                reason = "粗路径方向分段索引或起始换向标记无效。";
                return false;
            }

            for (int pointIndex = segment.StartIndex; pointIndex <= segment.EndIndex; pointIndex++)
            {
                if (path[pointIndex].Direction != segment.Direction)
                {
                    reason = "粗路径方向分段包含不同方向的点。";
                    return false;
                }
            }

            bool hasNextSegment = segmentIndex + 1 < segments.Count;
            bool expectedEndsAtGearSwitch = hasNextSegment && segment.EndIndex + 1 < path.Count &&
                path[segment.EndIndex + 1].IsGearSwitchPoint;
            if (segment.EndsAtGearSwitch != expectedEndsAtGearSwitch)
            {
                reason = "粗路径方向分段末尾换向标记无效。";
                return false;
            }
            expectedStartIndex = segment.EndIndex + 1;
        }

        if (expectedStartIndex != path.Count)
        {
            reason = "粗路径方向分段未完整覆盖全部点。";
            return false;
        }
        return true;
    }

    private static bool IsValidPoint(CoarsePathPoint point)
    {
        return point != null && NumericGuard.IsFinite(point.X) && NumericGuard.IsFinite(point.Y) &&
            NumericGuard.IsFinite(point.Heading) && NumericGuard.IsFinite(point.UnwrappedHeading) &&
            NumericGuard.IsFinite(point.ArcLength) && point.ArcLength >= 0d &&
            NumericGuard.IsFinite(point.VehicleCurvature) && NumericGuard.IsFinite(point.BodyClearance) &&
            point.BodyClearance >= 0d &&
            (point.Direction == TravelDirection.Forward || point.Direction == TravelDirection.Reverse) &&
            Enum.IsDefined(typeof(CoarsePathPointSource), point.Source) &&
            Math.Abs(AngleMath.ShortestSignedDifference(point.Heading, AngleMath.NormalizeRadians(point.Heading))) <= Tolerance;
    }
}