From 69ef08dff5e07df17bd1f7c55ec47b79a48ba12a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Wed, 29 Jul 2026 09:18:01 +0800 Subject: [PATCH] feat: validate smoothed vehicle paths --- .../Validation/SmoothedPathValidator.cs | 243 ++++++++++++++++++ .../verify_path_smoothing_validation.ps1 | 168 ++++++++++++ 2 files changed, 411 insertions(+) create mode 100644 ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Validation/SmoothedPathValidator.cs create mode 100644 ClumsyPilot/tests/verify_path_smoothing_validation.ps1 diff --git a/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Validation/SmoothedPathValidator.cs b/ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Validation/SmoothedPathValidator.cs new file mode 100644 index 0000000..410a753 --- /dev/null +++ b/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; + +/// 独立复核平滑候选的端点拓扑、车辆曲率和连续车体安全性。 +public sealed class SmoothedPathValidator +{ + private const double Tolerance = 1e-6d; + private readonly FootprintCollisionChecker _collisionChecker; + + /// 创建使用默认连续车体碰撞检查器的平滑路径验证器。 + public SmoothedPathValidator() + : this(new FootprintCollisionChecker()) + { + } + + /// 创建使用指定连续车体碰撞检查器的平滑路径验证器。 + public SmoothedPathValidator(FootprintCollisionChecker collisionChecker) + { + _collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker)); + } + + /// + /// 复核候选平滑路径。每个候选方向段必须保持原始段的端点和换向拓扑； + /// 输出中的净空均由本次实际车体检查重新计算，绝不沿用候选声明值。 + /// + public bool TryValidate( + IReadOnlyList candidatePath, + IReadOnlyList candidateSegments, + PreparedPath originalPath, + PlanningGridMap map, + VehicleParameters vehicle, + double maximumCollisionCheckStepMeters, + out IReadOnlyList 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(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(output); + minimumClearanceMeters = minimumClearance; + return true; + } + + private static bool TryValidateSegmentTopology( + IReadOnlyList candidatePath, + IReadOnlyList 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 EmptyPath() + { + return new ReadOnlyCollection(new List()); + } +} diff --git a/ClumsyPilot/tests/verify_path_smoothing_validation.ps1 b/ClumsyPilot/tests/verify_path_smoothing_validation.ps1 new file mode 100644 index 0000000..06d923a --- /dev/null +++ b/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.'