feat: validate smoothed vehicle paths

ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Validation/SmoothedPathValidator.cs

@@ -0,0 +1,243 @@
+using System;
+using System.Collections.Generic;
+using System.Collections.ObjectModel;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing.Processing;
+using MultiWheelC.TrajectoryPlanning.Utils;
+
+namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.Validation;
+
+/// <summary>独立复核平滑候选的端点拓扑、车辆曲率和连续车体安全性。</summary>
+public sealed class SmoothedPathValidator
+{
+    private const double Tolerance = 1e-6d;
+    private readonly FootprintCollisionChecker _collisionChecker;
+
+    /// <summary>创建使用默认连续车体碰撞检查器的平滑路径验证器。</summary>
+    public SmoothedPathValidator()
+        : this(new FootprintCollisionChecker())
+    {
+    }
+
+    /// <summary>创建使用指定连续车体碰撞检查器的平滑路径验证器。</summary>
+    public SmoothedPathValidator(FootprintCollisionChecker collisionChecker)
+    {
+        _collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
+    }
+
+    /// <summary>
+    /// 复核候选平滑路径。每个候选方向段必须保持原始段的端点和换向拓扑；
+    /// 输出中的净空均由本次实际车体检查重新计算，绝不沿用候选声明值。
+    /// </summary>
+    public bool TryValidate(
+        IReadOnlyList<SmoothedPathPoint> candidatePath,
+        IReadOnlyList<SmoothedPathSegment> candidateSegments,
+        PreparedPath originalPath,
+        PlanningGridMap map,
+        VehicleParameters vehicle,
+        double maximumCollisionCheckStepMeters,
+        out IReadOnlyList<SmoothedPathPoint> pathWithClearance,
+        out double minimumClearanceMeters,
+        out string reason)
+    {
+        pathWithClearance = EmptyPath();
+        minimumClearanceMeters = 0d;
+        reason = string.Empty;
+        if (candidatePath == null || candidateSegments == null || originalPath == null || map == null || vehicle == null ||
+            candidatePath.Count == 0 || candidateSegments.Count == 0 || !NumericGuard.IsPositiveFinite(maximumCollisionCheckStepMeters))
+        {
+            reason = "平滑候选、原始路径、地图、车辆或碰撞步长无效。";
+            return false;
+        }
+
+        if (!VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out double maximumCurvatureMeters))
+        {
+            reason = "车辆曲率约束无效。";
+            return false;
+        }
+
+        if (!TryValidateSegmentTopology(candidatePath, candidateSegments, originalPath, out reason)) return false;
+        if (Math.Abs(candidatePath[0].ArcLength) > Tolerance)
+        {
+            reason = "平滑路径首点的全局弧长必须为零。";
+            return false;
+        }
+
+        var checkedClearances = new double[candidatePath.Count];
+        double minimumClearance = double.PositiveInfinity;
+        for (int index = 0; index < candidatePath.Count; index++)
+        {
+            SmoothedPathPoint current = candidatePath[index];
+            if (!IsValidPoint(current) || Math.Abs(current.VehicleCurvature) > maximumCurvatureMeters + Tolerance)
+            {
+                reason = "平滑路径包含非法数值或超限车辆曲率。";
+                return false;
+            }
+
+            var currentPose = new Pose2D(current.X, current.Y, current.Heading);
+            if (!_collisionChecker.IsPoseCollisionFree(currentPose, map, vehicle, 0d, out double poseClearance))
+            {
+                reason = "平滑路径点未通过完整车体碰撞或边界复核。";
+                return false;
+            }
+
+            checkedClearances[index] = poseClearance;
+            minimumClearance = Math.Min(minimumClearance, poseClearance);
+            if (index == 0) continue;
+
+            SmoothedPathPoint previous = candidatePath[index - 1];
+            if (!IsUnwrappedHeadingContinuous(previous, current))
+            {
+                reason = "平滑路径展开航向不连续。";
+                return false;
+            }
+
+            if (IsDuplicatePoseAndArcLength(previous, current))
+            {
+                if (previous.Direction == current.Direction || !current.IsGearSwitchPoint)
+                {
+                    reason = "相邻重复点不是合法换向对。";
+                    return false;
+                }
+                continue;
+            }
+
+            if (current.IsGearSwitchPoint || current.ArcLength <= previous.ArcLength + Tolerance)
+            {
+                reason = "非换向点必须保持正弧长增量，换向点必须保留重复位姿。";
+                return false;
+            }
+
+            var previousPose = new Pose2D(previous.X, previous.Y, previous.Heading);
+            if (!_collisionChecker.IsSweptMotionCollisionFree(previousPose, currentPose, map, vehicle,
+                maximumCollisionCheckStepMeters, out double sweptClearance))
+            {
+                reason = "平滑路径相邻点之间的完整车体扫掠碰撞复核失败。";
+                return false;
+            }
+
+            checkedClearances[index - 1] = Math.Min(checkedClearances[index - 1], sweptClearance);
+            checkedClearances[index] = Math.Min(checkedClearances[index], sweptClearance);
+            minimumClearance = Math.Min(minimumClearance, sweptClearance);
+        }
+
+        var output = new List<SmoothedPathPoint>(candidatePath.Count);
+        for (int index = 0; index < candidatePath.Count; index++)
+        {
+            SmoothedPathPoint point = candidatePath[index];
+            output.Add(new SmoothedPathPoint(
+                point.X, point.Y, point.Heading, point.UnwrappedHeading, point.ArcLength, point.Direction,
+                point.GeometricCurvature, point.VehicleCurvature, checkedClearances[index], point.IsGearSwitchPoint, point.Source));
+        }
+
+        pathWithClearance = new ReadOnlyCollection<SmoothedPathPoint>(output);
+        minimumClearanceMeters = minimumClearance;
+        return true;
+    }
+
+    private static bool TryValidateSegmentTopology(
+        IReadOnlyList<SmoothedPathPoint> candidatePath,
+        IReadOnlyList<SmoothedPathSegment> candidateSegments,
+        PreparedPath originalPath,
+        out string reason)
+    {
+        reason = string.Empty;
+        if (originalPath.Segments == null || originalPath.Segments.Count != candidateSegments.Count)
+        {
+            reason = "平滑路径方向段数量必须保持原始换向拓扑。";
+            return false;
+        }
+
+        int expectedStartIndex = 0;
+        for (int segmentIndex = 0; segmentIndex < candidateSegments.Count; segmentIndex++)
+        {
+            SmoothedPathSegment candidateSegment = candidateSegments[segmentIndex];
+            PreparedDirectionSegment originalSegment = originalPath.Segments[segmentIndex];
+            if (candidateSegment == null || originalSegment == null || candidateSegment.SegmentIndex != segmentIndex ||
+                candidateSegment.StartIndex != expectedStartIndex || candidateSegment.StartIndex < 0 ||
+                candidateSegment.EndIndex < candidateSegment.StartIndex || candidateSegment.EndIndex >= candidatePath.Count ||
+                candidateSegment.Direction != originalSegment.Direction ||
+                candidateSegment.StartsAtGearSwitch != originalSegment.StartsAtGearSwitch ||
+                candidateSegment.EndsAtGearSwitch != originalSegment.EndsAtGearSwitch ||
+                originalSegment.Points == null || originalSegment.Points.Count == 0)
+            {
+                reason = "平滑路径方向段索引、方向或换向拓扑无效。";
+                return false;
+            }
+
+            SmoothedPathPoint candidateStart = candidatePath[candidateSegment.StartIndex];
+            SmoothedPathPoint candidateEnd = candidatePath[candidateSegment.EndIndex];
+            SmoothingPoint2D originalStart = originalSegment.Points[0];
+            SmoothingPoint2D originalEnd = originalSegment.Points[originalSegment.Points.Count - 1];
+            if (!SamePose(candidateStart, originalStart) || !SamePose(candidateEnd, originalEnd) ||
+                candidateStart.Direction != originalSegment.Direction || candidateEnd.Direction != originalSegment.Direction ||
+                candidateStart.IsGearSwitchPoint != originalStart.IsGearSwitchPoint ||
+                candidateEnd.IsGearSwitchPoint != originalEnd.IsGearSwitchPoint)
+            {
+                reason = "平滑路径改变了原始方向段端点或换向点。";
+                return false;
+            }
+
+            for (int pointIndex = candidateSegment.StartIndex; pointIndex <= candidateSegment.EndIndex; pointIndex++)
+            {
+                if (candidatePath[pointIndex] == null || candidatePath[pointIndex].Direction != originalSegment.Direction)
+                {
+                    reason = "平滑路径方向段包含与段方向不一致的点。";
+                    return false;
+                }
+            }
+
+            expectedStartIndex = candidateSegment.EndIndex + 1;
+        }
+
+        if (expectedStartIndex != candidatePath.Count)
+        {
+            reason = "平滑路径方向段未完整覆盖候选点。";
+            return false;
+        }
+        return true;
+    }
+
+    private static bool IsValidPoint(SmoothedPathPoint 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.GeometricCurvature) && NumericGuard.IsFinite(point.VehicleCurvature) &&
+            IsDirection(point.Direction) && Enum.IsDefined(typeof(SmoothedPathPointSource), point.Source) &&
+            Math.Abs(AngleMath.ShortestSignedDifference(point.Heading, AngleMath.NormalizeRadians(point.Heading))) <= Tolerance;
+    }
+
+    private static bool SamePose(SmoothedPathPoint candidate, SmoothingPoint2D original)
+    {
+        return candidate != null && original != null && Math.Abs(candidate.X - original.X) <= Tolerance &&
+            Math.Abs(candidate.Y - original.Y) <= Tolerance &&
+            Math.Abs(AngleMath.ShortestSignedDifference(candidate.Heading, original.Heading)) <= Tolerance;
+    }
+
+    private static bool IsUnwrappedHeadingContinuous(SmoothedPathPoint previous, SmoothedPathPoint current)
+    {
+        double expectedDelta = AngleMath.ShortestSignedDifference(previous.Heading, current.Heading);
+        return NumericGuard.IsFinite(expectedDelta) &&
+            Math.Abs((current.UnwrappedHeading - previous.UnwrappedHeading) - expectedDelta) <= Tolerance;
+    }
+
+    private static bool IsDuplicatePoseAndArcLength(SmoothedPathPoint previous, SmoothedPathPoint current)
+    {
+        return Math.Abs(previous.X - current.X) <= Tolerance && Math.Abs(previous.Y - current.Y) <= Tolerance &&
+            Math.Abs(AngleMath.ShortestSignedDifference(previous.Heading, current.Heading)) <= Tolerance &&
+            Math.Abs(previous.ArcLength - current.ArcLength) <= Tolerance;
+    }
+
+    private static bool IsDirection(TravelDirection direction)
+    {
+        return direction == TravelDirection.Forward || direction == TravelDirection.Reverse;
+    }
+
+    private static IReadOnlyList<SmoothedPathPoint> EmptyPath()
+    {
+        return new ReadOnlyCollection<SmoothedPathPoint>(new List<SmoothedPathPoint>());
+    }
+}

ClumsyPilot/tests/verify_path_smoothing_validation.ps1

@@ -0,0 +1,168 @@
+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-False($Actual, [string]$Message) {
+    if ($Actual) { throw $Message }
+}
+
+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)
+}
+
+function New-Map([bool]$WithObstacle) {
+    $bounds = [Activator]::CreateInstance($boundsType, @([single]0, [single]4000, [single]0, [single]4000))
+    $request = [Activator]::CreateInstance($mapRequestType)
+    $request.Bounds = $bounds
+    $request.ResolutionMm = [single]50
+    if ($WithObstacle) {
+        $obstacle = [Activator]::CreateInstance($rectangleType, @([single]1900, [single]2100, [single]1800, [single]2200))
+        $obstacles = [Array]::CreateInstance($obstacleType, 1)
+        $obstacles.SetValue($obstacle, 0)
+        $source = [Activator]::CreateInstance($manualSourceType, @('validator-obstacle', [long]1, $true, $obstacles))
+        $sources = [Array]::CreateInstance($obstacleSourceType, 1)
+        $sources.SetValue($source, 0)
+        $request.ObstacleSources = $sources
+    } else {
+        $request.ObstacleSources = [Array]::CreateInstance($obstacleSourceType, 0)
+        $request.AllowExplicitEmptyMap = $true
+    }
+    $result = [Activator]::CreateInstance($mapFactoryType).Create($request)
+    Assert-True $result.Succeeded 'Validation test map must be built.'
+    Assert-True $result.Map.PlanningReady 'Validation test map must be ready.'
+    return $result.Map
+}
+
+function New-SmoothedPoint([double]$X, [double]$Y, [double]$ArcLength, $Direction,
+    [double]$VehicleCurvature = 0.0, [bool]$IsGearSwitch = $false, [double]$Clearance = 999.0) {
+    return [Activator]::CreateInstance($smoothedPointType, @(
+        $X, $Y, [double]0.0, [double]0.0, $ArcLength, $Direction,
+        $VehicleCurvature, $VehicleCurvature, $Clearance, $IsGearSwitch, $anchor))
+}
+
+function New-PreparedPoint([double]$X, [double]$Y, [double]$ArcLength, [bool]$IsGearSwitch = $false) {
+    return [Activator]::CreateInstance($smoothingPointType, @(
+        $X, $Y, $ArcLength, [double]0.0, [double]0.0, [double]999.0, $IsGearSwitch, $anchor))
+}
+
+function New-OneSegmentCase([double]$X0, [double]$Y0, [double]$X1, [double]$Y1, [double]$VehicleCurvature = 0.0,
+    [double]$StartArcLength = 0.0) {
+    $candidatePath = [Array]::CreateInstance($smoothedPointType, 2)
+    $candidatePath.SetValue((New-SmoothedPoint $X0 $Y0 $StartArcLength $forward), 0)
+    $candidatePath.SetValue((New-SmoothedPoint $X1 $Y1 ($StartArcLength + 1.0) $forward $VehicleCurvature), 1)
+    $candidateSegments = [Array]::CreateInstance($smoothedSegmentType, 1)
+    $candidateSegments.SetValue([Activator]::CreateInstance($smoothedSegmentType, @(0, $forward, 0, 1, $false, $false)), 0)
+    $preparedPoints = [Array]::CreateInstance($smoothingPointType, 2)
+    $preparedPoints.SetValue((New-PreparedPoint $X0 $Y0 0.0), 0)
+    $preparedPoints.SetValue((New-PreparedPoint $X1 $Y1 1.0), 1)
+    $preparedSegments = [Array]::CreateInstance($preparedSegmentType, 1)
+    $preparedSegments.SetValue([Activator]::CreateInstance($preparedSegmentType, @(0, $forward, $preparedPoints, $false, $false)), 0)
+    return [PSCustomObject]@{
+        CandidatePath = $candidatePath
+        CandidateSegments = $candidateSegments
+        Original = [Activator]::CreateInstance($preparedPathType, [object[]]@(,$preparedSegments))
+    }
+}
+
+function Invoke-Validation($Case, $Map) {
+    $arguments = [object[]]@($Case.CandidatePath, $Case.CandidateSegments, $Case.Original, $Map, $vehicle,
+        [double]0.05, $null, [double]0.0, $null)
+    $accepted = $validateMethod.Invoke($validator, $arguments)
+    return [PSCustomObject]@{ Accepted = $accepted; Path = $arguments[6]; MinimumClearance = $arguments[7]; Reason = $arguments[8] }
+}
+
+function New-MovedGearSwitchCase {
+    $candidatePath = [Array]::CreateInstance($smoothedPointType, 4)
+    $candidatePath.SetValue((New-SmoothedPoint 0.5 0.5 0.0 $forward), 0)
+    $candidatePath.SetValue((New-SmoothedPoint 1.5 0.5 1.0 $forward), 1)
+    $candidatePath.SetValue((New-SmoothedPoint 1.6 0.5 1.0 $reverse 0.0 $true), 2)
+    $candidatePath.SetValue((New-SmoothedPoint 0.5 0.5 2.0 $reverse), 3)
+    $candidateSegments = [Array]::CreateInstance($smoothedSegmentType, 2)
+    $candidateSegments.SetValue([Activator]::CreateInstance($smoothedSegmentType, @(0, $forward, 0, 1, $false, $true)), 0)
+    $candidateSegments.SetValue([Activator]::CreateInstance($smoothedSegmentType, @(1, $reverse, 2, 3, $true, $false)), 1)
+    $preparedFirst = [Array]::CreateInstance($smoothingPointType, 2)
+    $preparedFirst.SetValue((New-PreparedPoint 0.5 0.5 0.0), 0)
+    $preparedFirst.SetValue((New-PreparedPoint 1.5 0.5 1.0), 1)
+    $preparedSecond = [Array]::CreateInstance($smoothingPointType, 2)
+    $preparedSecond.SetValue((New-PreparedPoint 1.5 0.5 0.0 $true), 0)
+    $preparedSecond.SetValue((New-PreparedPoint 0.5 0.5 1.0), 1)
+    $preparedSegments = [Array]::CreateInstance($preparedSegmentType, 2)
+    $preparedSegments.SetValue([Activator]::CreateInstance($preparedSegmentType, @(0, $forward, $preparedFirst, $false, $true)), 0)
+    $preparedSegments.SetValue([Activator]::CreateInstance($preparedSegmentType, @(1, $reverse, $preparedSecond, $true, $false)), 1)
+    return [PSCustomObject]@{
+        CandidatePath = $candidatePath
+        CandidateSegments = $candidateSegments
+        Original = [Activator]::CreateInstance($preparedPathType, [object[]]@(,$preparedSegments))
+    }
+}
+
+$root = 'MultiWheelC.TrajectoryPlanning.PathSmoothing.'
+$processing = $root + 'Processing.'
+$validation = $root + 'Validation.'
+$coarsePath = 'MultiWheelC.TrajectoryPlanning.CoarsePath.'
+$mapping = 'MultiWheelC.TrajectoryPlanning.Mapping.'
+
+$validatorType = Get-RequiredType ($validation + 'SmoothedPathValidator')
+$smoothedPointType = Get-RequiredType ($root + 'SmoothedPathPoint')
+$smoothedSegmentType = Get-RequiredType ($root + 'SmoothedPathSegment')
+$smoothingPointType = Get-RequiredType ($processing + 'SmoothingPoint2D')
+$preparedSegmentType = Get-RequiredType ($processing + 'PreparedDirectionSegment')
+$preparedPathType = Get-RequiredType ($processing + 'PreparedPath')
+$directionType = Get-RequiredType ($coarsePath + 'TravelDirection')
+$sourceType = Get-RequiredType ($root + 'SmoothedPathPointSource')
+$vehicleType = Get-RequiredType ($coarsePath + 'VehicleParameters')
+$boundsType = Get-RequiredType ($mapping + 'MapBoundsMm')
+$obstacleType = Get-RequiredType ($mapping + 'IMapObstacle')
+$rectangleType = Get-RequiredType ($mapping + 'AxisAlignedRectangleObstacle')
+$obstacleSourceType = Get-RequiredType ($mapping + 'IMapObstacleSource')
+$manualSourceType = Get-RequiredType ($mapping + 'ManualObstacleSource')
+$mapRequestType = Get-RequiredType ($mapping + 'PlanningMapRequest')
+$mapFactoryType = Get-RequiredType ($mapping + 'PlanningMapFactory')
+
+$validator = [Activator]::CreateInstance($validatorType)
+$validateMethod = $validatorType.GetMethod('TryValidate')
+Assert-True ($null -ne $validateMethod) 'SmoothedPathValidator must expose TryValidate.'
+Assert-True ($validateMethod.GetParameters().Length -eq 9) 'TryValidate must accept path, segments, originals, map, vehicle, step, path, clearance, and reason.'
+$forward = [Enum]::Parse($directionType, 'Forward')
+$reverse = [Enum]::Parse($directionType, 'Reverse')
+$anchor = [Enum]::Parse($sourceType, 'Anchor')
+$vehicle = [Activator]::CreateInstance($vehicleType)
+$vehicle.LengthMeters = 0.20
+$vehicle.WidthMeters = 0.20
+$vehicle.SafetyMarginMeters = 0.0
+$vehicle.MaximumCurvaturePerMeter = 1.0
+$vehicle.MinimumTurningRadiusMeters = 1.0
+
+$emptyMap = New-Map $false
+$obstacleMap = New-Map $true
+$valid = Invoke-Validation (New-OneSegmentCase 0.5 0.5 1.5 0.5) $obstacleMap
+Assert-True $valid.Accepted ('A valid straight candidate must pass. Reason=' + $valid.Reason)
+Assert-True ($null -ne $valid.Path) 'A valid candidate must return clearance-recomputed points.'
+Assert-True ($valid.Path[0].BodyClearance -lt 999.0) 'Validated output must replace an overclaimed candidate clearance.'
+Assert-True ($valid.MinimumClearance -ge 0.0) 'A valid candidate must report non-negative conservative clearance.'
+
+$pointCollision = Invoke-Validation (New-OneSegmentCase 2.0 2.0 2.5 2.0) $obstacleMap
+Assert-False $pointCollision.Accepted 'A smoothing candidate that touches an obstacle must be rejected.'
+$sweptCollision = Invoke-Validation (New-OneSegmentCase 1.5 2.0 2.5 2.0) $obstacleMap
+Assert-False $sweptCollision.Accepted 'A smoothing candidate whose sweep cuts through an obstacle must be rejected.'
+$outsideBounds = Invoke-Validation (New-OneSegmentCase 0.05 0.5 0.5 0.5) $emptyMap
+Assert-False $outsideBounds.Accepted 'A smoothing candidate outside map bounds must be rejected.'
+$overCurvature = Invoke-Validation (New-OneSegmentCase 0.5 0.5 1.5 0.5 2.0) $emptyMap
+Assert-False $overCurvature.Accepted 'A smoothing candidate above vehicle maximum curvature must be rejected.'
+$nonzeroStartArc = Invoke-Validation (New-OneSegmentCase 0.5 0.5 1.5 0.5 0.0 1.0) $emptyMap
+Assert-False $nonzeroStartArc.Accepted 'A smoothing candidate must start at global arc length zero.'
+$movedSwitch = Invoke-Validation (New-MovedGearSwitchCase) $emptyMap
+Assert-False $movedSwitch.Accepted 'A smoothing candidate with a moved gear-switch pose must be rejected.'
+
+Write-Output 'Path smoothing validation checks passed.'