46 KiB
Path Smoothing Comparison Implementation Plan
For agentic workers: REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (
- [ ]) syntax for tracking.
Goal: Build a validated coarse-path smoothing module that compares cubic B-spline, local cubic Bézier, and piecewise quintic paths, then exports reproducible IEEE-style Chinese reports for existing and fast fixture scenarios.
Architecture: Add PathSmoothing beside CoarsePath; keep contracts, shared geometry processing, algorithms, validation, comparison, and rendering isolated. Every algorithm consumes the same preprocessed direction segments, every candidate passes the same analyzer and full-footprint validator, and only accepted results reach the formal facade. Reporting consumes immutable comparison data and cannot affect planning status.
Tech Stack: C# 10, .NET Standard 2.0, existing PlanningGridMap/FootprintCollisionChecker, PowerShell reflection tests, Newtonsoft.Json 13.0.4, StbImageWriteSharp 1.16.7, System.Drawing.Common 10.0.10 on Windows.
Global Constraints
- Core units are m, rad, and 1/m; no mm or degree conversion inside smoothing algorithms.
- Preserve start, goal, direction-segment order, gear-switch count, and gear-switch poses exactly.
- Never differentiate, resample, or fit across a gear-switch duplicate pair.
- Default output spacing is
0.05 m; default swept-collision step is0.025 m. - A formal result publishes a path only for
Successor explicitly verifiedFallbackToCoarsePath. - Rejected candidates may appear only in comparison diagnostics, never in
PathSmoothingResult.Path. - No speed, acceleration, time, SQP, Frenet, chassis, sensor, localization, or UI dependencies.
- Figure size is
7.16 × 5.2 in; PNG size is2148 × 1560 pxwith 300 dpi metadata. - Chinese text uses
SimSun; English, numbers, Greek, and mathematics useTimes New Roman. - Fixed method colors are raw
#4D4D4D, B-spline#0072B2, Bézier#D55E00, quintic#009E73, and curvature limits#CC79A7. System.Drawing.Commonis a Windows-only report-rendering dependency; smoothing, validation, comparison, SVG, and CSV remain independent of its runtime availability.- Follow TDD for every task: failing verification first, minimal implementation second, full relevant verification before commit.
Task 1: Immutable smoothing contracts
Files:
- Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothingMethod.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingStatus.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathPointSource.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathPoint.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathSegment.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/CubicBSplineOptions.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/LocalCubicBezierOptions.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PiecewiseQuinticOptions.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingConfiguration.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRequest.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathQualityMetrics.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingDiagnostics.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingResult.cs - Test:
ClumsyPilot/tests/verify_path_smoothing_contracts.ps1
Interfaces:
-
Consumes:
CoarsePathPoint,PathSegment,PlanningGridMap,VehicleParameters. -
Produces:
PathSmoothingRequest;PathSmoothingResult.Success(...);PathSmoothingResult.Fallback(...);PathSmoothingResult.Failure(...); all public enums and value objects used below. -
Step 1: Write the failing contract verification
Create a reflection script that loads ClumsyPilot.dll, resolves every type listed above, and verifies these exact defaults and invariants:
$configuration = [Activator]::CreateInstance($configurationType)
Assert-Near 0.05 $configuration.OutputSpacingMeters 'Default output spacing must be 0.05 m.'
Assert-Near 0.025 $configuration.MaximumCollisionCheckStepMeters 'Default collision step must be 0.025 m.'
Assert-Equal 4 $configuration.RetryStrengthScales.Count 'Retry schedule must contain four entries.'
Assert-Near 1.0 $configuration.RetryStrengthScales[0] 'First retry scale must be 1.0.'
Assert-Near 0.25 $configuration.RetryStrengthScales[3] 'Last retry scale must be 0.25.'
$failed = $resultType.GetMethod('Failure').Invoke(
$null,
@([Enum]::Parse($statusType, 'InvalidInput'),
[Activator]::CreateInstance($diagnosticsType)))
Assert-Equal 0 $failed.Path.Count 'Failure must publish no path.'
Assert-Equal 0 $failed.Segments.Count 'Failure must publish no segments.'
Also construct a SmoothedPathPoint and assert all units/properties, construct two SmoothedPathSegment instances, and prove returned collections cannot be mutated.
- Step 2: Run the verification and confirm RED
Run:
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_contracts.ps1
Expected: build succeeds on existing code; script fails because MultiWheelC.TrajectoryPlanning.PathSmoothing.PathSmoothingConfiguration is missing.
- Step 3: Implement enums, immutable values, and result factories
Use namespace MultiWheelC.TrajectoryPlanning.PathSmoothing. Implement these exact public shapes:
public enum SmoothingMethod { CubicBSpline, LocalCubicBezier, PiecewiseQuintic }
public enum PathSmoothingStatus { Success, FallbackToCoarsePath, InvalidInput, Infeasible, Failed, Cancelled }
public enum SmoothedPathPointSource { Anchor, Interpolated, GearSwitch, CoarsePathFallback }
public sealed class PathSmoothingConfiguration
{
public PathSmoothingConfiguration()
{
OutputSpacingMeters = 0.05d;
MaximumCollisionCheckStepMeters = 0.025d;
MinimumClearanceReserveMeters = 0.02d;
SmoothingStrength = 1d;
AllowFallbackToCoarsePath = true;
RetryStrengthScales = new ReadOnlyCollection<double>(
new List<double> { 1d, 0.75d, 0.50d, 0.25d });
}
public SmoothingMethod Method { get; set; }
public double OutputSpacingMeters { get; set; }
public double MaximumCollisionCheckStepMeters { get; set; }
public double MinimumClearanceReserveMeters { get; set; }
public double SmoothingStrength { get; set; }
public bool AllowFallbackToCoarsePath { get; set; }
public IReadOnlyList<double> RetryStrengthScales { get; }
public CubicBSplineOptions CubicBSpline { get; } = new CubicBSplineOptions();
public LocalCubicBezierOptions LocalCubicBezier { get; } = new LocalCubicBezierOptions();
public PiecewiseQuinticOptions PiecewiseQuintic { get; } = new PiecewiseQuinticOptions();
}
Use these exact option defaults:
public sealed class CubicBSplineOptions
{
public double EndpointTangentScale { get; set; } = 1d / 3d;
}
public sealed class LocalCubicBezierOptions
{
public double CornerHeadingThresholdRadians { get; set; } = Math.PI / 18d;
public double MaximumWindowLengthMeters { get; set; } = 0.60d;
public double HandleLengthRatio { get; set; } = 1d / 3d;
}
public sealed class PiecewiseQuinticOptions
{
public double KnotSpacingMeters { get; set; } = 0.50d;
public double MinimumKnotSpacingMeters { get; set; } = 0.10d;
}
Define the request constructor exactly as:
public PathSmoothingRequest(
IReadOnlyList<CoarsePathPoint> coarsePath,
IReadOnlyList<PathSegment> segments,
PlanningGridMap map,
VehicleParameters vehicle,
PathSmoothingConfiguration configuration)
Define parameterless immutable-empty defaults for PathQualityMetrics and PathSmoothingDiagnostics, plus overloads that accept all measured values. The three result factories are:
public static PathSmoothingResult Success(
SmoothingMethod method, IReadOnlyList<SmoothedPathPoint> path,
IReadOnlyList<SmoothedPathSegment> segments, PathSmoothingDiagnostics diagnostics);
public static PathSmoothingResult Fallback(
SmoothingMethod attemptedMethod, IReadOnlyList<SmoothedPathPoint> path,
IReadOnlyList<SmoothedPathSegment> segments, PathSmoothingDiagnostics diagnostics);
public static PathSmoothingResult Failure(
PathSmoothingStatus status, PathSmoothingDiagnostics diagnostics);
PathSmoothingResult must copy all input lists into ReadOnlyCollection<T>. Reject Success/Fallback factories with empty paths or segments; reject Failure with a success-like status.
- Step 4: Run contract verification and existing integration verification
Run:
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_contracts.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_integration.ps1
Expected: both scripts end with their passed messages.
- Step 5: Commit
git add -- ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts ClumsyPilot/tests/verify_path_smoothing_contracts.ps1
git commit -m "feat: define path smoothing contracts"
Task 2: Input preparation, direction splitting, resampling, and geometry analysis
Files:
- Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PreparedPath.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PreparedDirectionSegment.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/SmoothingPoint2D.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalysis.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathSmoothingPreprocessor.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/ArcLengthResampler.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalyzer.cs - Test:
ClumsyPilot/tests/verify_path_smoothing_geometry.ps1
Interfaces:
-
Consumes:
PathSmoothingRequest. -
Produces:
PathSmoothingPreprocessor.TryPrepare(request, out PreparedPath, out string)andPathGeometryAnalyzer.TryAnalyze(candidateSegments, spacing, out PathGeometryAnalysis analysis, out string reason). -
Step 1: Write analytic failing tests
Build reflection helpers that create:
forward straight: (0,0,0) → (1,0,0), expected κ = 0
forward quarter circle: R=2, expected vehicle κ = +0.5
reverse quarter circle: R=2, expected vehicle κ = -0.5
gear switch: duplicated pose and arc length with opposite directions
Assert 0.05 m spacing, exact final point, continuous UnwrappedHeading, correct curvature signs, and no derivative across the gear-switch pair.
- Step 2: Run and confirm RED
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_geometry.ps1
Expected: fails because PathGeometryAnalyzer is missing.
- Step 3: Implement deterministic preparation and resampling
Implement segment-local linear arc interpolation:
double ratio = (targetArc - left.ArcLength) / (right.ArcLength - left.ArcLength);
double x = left.X + ratio * (right.X - left.X);
double y = left.Y + ratio * (right.Y - left.Y);
double heading = left.UnwrappedHeading +
ratio * (right.UnwrappedHeading - left.UnwrappedHeading);
Always append the exact segment end. Preserve separate gear-switch endpoints. Reject non-finite values, non-monotonic arc length, illegal duplicates, and segment coverage gaps.
- Step 4: Implement shared geometry formulas
For each direction segment, recompute arc length from Euclidean position increments, unwrap heading from travel tangents, and use stable one-sided/central differences:
double geometricCurvature = deltaHeading / deltaArc;
double directionSign = direction == TravelDirection.Forward ? 1d : -1d;
double vehicleCurvature = directionSign * geometricCurvature;
double totalVariationIncrement = Math.Abs(currentCurvature - previousCurvature);
double variationEnergyIncrement =
Math.Pow((currentCurvature - previousCurvature) / deltaArc, 2d) * deltaArc;
At segment boundaries use only samples from that segment. Never include the gear-switch duplicate distance in a denominator.
- Step 5: Run geometry, contract, and utility checks
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_geometry.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_contracts.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_utils.ps1
Expected: all pass; circle curvature error stays within the tolerance encoded in the test.
- Step 6: Commit
git add -- ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing ClumsyPilot/tests/verify_path_smoothing_geometry.ps1
git commit -m "feat: add smoothing path geometry processing"
Task 3: Full-footprint smoothing validator
Files:
- Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Validation/SmoothedPathValidator.cs - Test:
ClumsyPilot/tests/verify_path_smoothing_validation.ps1
Interfaces:
-
Consumes: analyzed
SmoothedPathPoint/SmoothedPathSegment, original prepared endpoints,PlanningGridMap,VehicleParameters, collision step. -
Produces:
TryValidate(..., out IReadOnlyList<SmoothedPathPoint> pathWithClearance, out double minimumClearance, out string reason). -
Step 1: Write failing safety cases
Create an empty map, an occupied rectangle map, and paths that are: valid; point-colliding; swept-motion-colliding; outside bounds; over maximum curvature; changed at a gear switch; and overclaiming clearance.
Assert-False $collisionAccepted 'A smoothing candidate that cuts through an obstacle must be rejected.'
Assert-False $curvatureAccepted 'A smoothing candidate above vehicle maximum curvature must be rejected.'
Assert-False $switchAccepted 'A moved gear-switch pose must be rejected.'
Assert-True $validAccepted 'A valid straight candidate must pass.'
- Step 2: Run and confirm RED
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_validation.ps1
Expected: missing SmoothedPathValidator.
- Step 3: Implement validator using existing collision semantics
For every point call FootprintCollisionChecker.IsPoseCollisionFree; for every non-duplicate adjacent pair call IsSweptMotionCollisionFree. Enforce:
if (Math.Abs(point.VehicleCurvature) > maximumCurvature + 1e-6d)
return Fail("平滑路径车辆曲率超过车辆上限。");
if (!SamePose(candidateStart, originalStart) || !SamePose(candidateEnd, originalEnd))
return Fail("平滑路径改变了方向段端点。");
Replace candidate clearance with the conservative checked value; never retain a larger claimed value.
- Step 4: Run validator and existing collision checks
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_validation.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_collision.ps1
Expected: both pass.
- Step 5: Commit
git add -- ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Validation ClumsyPilot/tests/verify_path_smoothing_validation.ps1
git commit -m "feat: validate smoothed vehicle paths"
Task 4: Algorithm contract and finite retry runner
Files:
- Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/IPathSmoother.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/SmoothingCandidate.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/SmoothingAlgorithmInput.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/SmoothingAlgorithmRunner.cs - Test:
ClumsyPilot/tests/verify_path_smoothing_runner.ps1
Interfaces:
- Consumes:
PreparedPath, method-specificIPathSmoother, analyzer, validator, retry scales. - Produces: one accepted candidate or stable
Infeasible/Faileddiagnostics with attempted strengths and optional rejected comparison geometry.
Use this exact internal algorithm interface:
internal interface IPathSmoother
{
SmoothingMethod Method { get; }
SmoothingCandidate Smooth(
SmoothingAlgorithmInput input,
double effectiveStrength,
CancellationToken cancellationToken);
}
- Step 1: Write failing fake-smoother tests
Use reflection plus internal test doubles exposed from a nested test helper to prove:
strengths attempted exactly: 1.00, 0.75, 0.50, 0.25
stop immediately after first accepted candidate
do not retry numerical invalid input
cancel before the next attempt
retain rejected candidate only in comparison diagnostics
- Step 2: Run and confirm RED
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_runner.ps1
Expected: missing SmoothingAlgorithmRunner.
- Step 3: Implement the runner
Use this control flow:
foreach (double scale in configuration.RetryStrengthScales)
{
cancellationToken.ThrowIfCancellationRequested();
SmoothingCandidate candidate = smoother.Smooth(input, configuration.SmoothingStrength * scale);
if (!candidate.Succeeded) return AlgorithmRunResult.Failed(candidate.Reason);
if (!_analyzer.TryAnalyze(candidate.Segments, configuration.OutputSpacingMeters, out PathGeometryAnalysis analyzed, out var reason))
return AlgorithmRunResult.Failed(reason);
if (_validator.TryValidate(analyzed.Path, analyzed.Segments, input, out var safePath, out var clearance, out reason))
return AlgorithmRunResult.Success(safePath, analyzed.Segments, metrics, scale);
rejected = candidate;
failures.Add(reason);
}
return AlgorithmRunResult.Infeasible(rejected, failures);
Convert OperationCanceledException to Cancelled at the facade boundary, not inside numerical algorithms.
- Step 4: Run runner, geometry, and validator tests
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_runner.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_geometry.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_validation.ps1
Expected: all pass.
- Step 5: Commit
git add -- ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms ClumsyPilot/tests/verify_path_smoothing_runner.ps1
git commit -m "feat: add smoothing algorithm retry runner"
Task 5: Clamped cubic B-spline smoother
Files:
- Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/CubicBSplineSmoother.cs - Test:
ClumsyPilot/tests/verify_path_smoothing_bspline.ps1
Interfaces:
-
Implements:
IPathSmoother.Method == SmoothingMethod.CubicBSpline. -
Produces: one geometry-only candidate per input direction segment.
-
Step 1: Write failing deterministic shape tests
Verify a straight segment remains collinear, a five-point corner becomes tangent-continuous, endpoints and travel tangents are exact, and every displacement stays within its supplied movement radius.
- Step 2: Run and confirm RED
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_bspline.ps1
Expected: missing CubicBSplineSmoother.
- Step 3: Implement clamped basis evaluation and constrained control points
Use degree 3, endpoint knot multiplicity 4, and Cox–de Boor basis evaluation. Fix the first/last control points to endpoints; place adjacent control points on endpoint travel tangents. Blend remaining control points toward local three-point averages:
SmoothingPoint2D target = (previous + current + next) / 3d;
SmoothingPoint2D proposed = current + strength * (target - current);
SmoothingPoint2D bounded = ClampDisplacement(current, proposed, allowedRadius);
Return the original straight samples when all perpendicular deviations are below 1e-9.
- Step 4: Run algorithm and shared safety tests
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_bspline.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_runner.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_validation.ps1
Expected: all pass.
- Step 5: Commit
git add -- ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/CubicBSplineSmoother.cs ClumsyPilot/tests/verify_path_smoothing_bspline.ps1
git commit -m "feat: add cubic b-spline path smoother"
Task 6: Local cubic Bézier smoother
Files:
- Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/LocalCubicBezierSmoother.cs - Test:
ClumsyPilot/tests/verify_path_smoothing_bezier.ps1
Interfaces:
-
Implements:
IPathSmoother.Method == SmoothingMethod.LocalCubicBezier. -
Consumes: heading-change threshold, maximum local window, handle-length ratio.
-
Step 1: Write failing local-behavior tests
Assert no window is created for a straight line; one corner creates one replacement; overlapping windows merge; outside-window samples remain bitwise equal; endpoints and switch points remain fixed.
- Step 2: Run and confirm RED
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_bezier.ps1
Expected: missing LocalCubicBezierSmoother.
- Step 3: Implement window detection and Bézier evaluation
Detect corners from absolute change in travel tangent. Merge touching windows. For each window:
SmoothingPoint2D p1 = p0 + entryTangent * handleLength;
SmoothingPoint2D p2 = p3 - exitTangent * handleLength;
SmoothingPoint2D value =
Math.Pow(1d - t, 3d) * p0 +
3d * Math.Pow(1d - t, 2d) * t * p1 +
3d * (1d - t) * t * t * p2 +
t * t * t * p3;
Clamp each evaluated displacement to the interpolated movement radius and retain original samples outside merged windows.
- Step 4: Run Bézier and regression checks
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_bezier.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_runner.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_integration.ps1
Expected: all pass.
- Step 5: Commit
git add -- ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/LocalCubicBezierSmoother.cs ClumsyPilot/tests/verify_path_smoothing_bezier.ps1
git commit -m "feat: add local cubic bezier smoother"
Task 7: Piecewise quintic Hermite smoother
Files:
- Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/PiecewiseQuinticSmoother.cs - Test:
ClumsyPilot/tests/verify_path_smoothing_quintic.ps1
Interfaces:
-
Implements:
IPathSmoother.Method == SmoothingMethod.PiecewiseQuintic. -
Produces: C2-connected local polynomial segments without crossing direction boundaries.
-
Step 1: Write failing continuity and degeneracy tests
At every shared knot evaluate left/right position, first derivative, and second derivative; assert equality within 1e-6. Verify exact endpoint pose, bounded movement, and stable failure for a segment shorter than the configured minimum knot spacing.
- Step 2: Run and confirm RED
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_quintic.ps1
Expected: missing PiecewiseQuinticSmoother.
- Step 3: Implement normalized quintic Hermite basis
For each knot interval use normalized t ∈ [0,1] and solve the six coefficients independently for X and Y from:
p(0)=p0, p(1)=p1
p'(0)=v0, p'(1)=v1
p''(0)=a0, p''(1)=a1
Derive endpoint velocities from travel tangents times interval length. Blend shared accelerations once per knot and reuse the same value on both adjacent intervals. Reject singular or non-finite coefficients before sampling.
- Step 4: Run quintic, geometry, and validator checks
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_quintic.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_geometry.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_validation.ps1
Expected: all pass.
- Step 5: Commit
git add -- ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Algorithms/PiecewiseQuinticSmoother.cs ClumsyPilot/tests/verify_path_smoothing_quintic.ps1
git commit -m "feat: add piecewise quintic path smoother"
Task 8: Formal smoothing facade and explicit coarse-path fallback
Files:
- Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingService.cs - Test:
ClumsyPilot/tests/verify_path_smoothing_service.ps1
Interfaces:
-
Produces:
PathSmoothingResult Smooth(PathSmoothingRequest, CancellationToken). -
Consumes: preprocessor, method registry, retry runner, analyzer, validator.
-
Step 1: Write failing facade tests
Cover valid straight success, invalid input, cancellation, infeasible-without-fallback, and infeasible-with-verified-fallback. Assert fallback points use CoarsePathFallback and status never equals Success.
- Step 2: Run and confirm RED
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_service.ps1
Expected: missing PathSmoothingService.
- Step 3: Implement orchestration
Use a stable method registry:
private IPathSmoother Resolve(SmoothingMethod method) =>
method switch
{
SmoothingMethod.CubicBSpline => _bSpline,
SmoothingMethod.LocalCubicBezier => _bezier,
SmoothingMethod.PiecewiseQuintic => _quintic,
_ => throw new ArgumentOutOfRangeException(nameof(method)),
};
Validate the coarse path before smoothing. On allowed fallback, convert the revalidated coarse points to SmoothedPathPoint with CoarsePathFallback, re-run shared geometry analysis, and return Fallback, preserving the failed method diagnostics.
- Step 4: Run all core smoothing verifications
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_contracts.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_geometry.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_validation.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_service.ps1
Expected: all pass.
- Step 5: Commit
git add -- ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade ClumsyPilot/tests/verify_path_smoothing_service.ps1
git commit -m "feat: expose validated path smoothing service"
Task 9: Comparison metrics, isolation, and deterministic ranking
Files:
- Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Comparison/PathSmoothingComparisonRequest.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Comparison/PathSmoothingComparisonEntry.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Comparison/PathSmoothingComparisonResult.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Comparison/SmoothingMethodRanker.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingComparisonService.cs - Test:
ClumsyPilot/tests/verify_path_smoothing_comparison.ps1
Interfaces:
-
Produces:
Compare(PathSmoothingComparisonRequest, CancellationToken). -
Ranking order: feasible count; median variation energy; worst peak utilization; worst clearance loss; median length increase; median elapsed.
-
Step 1: Write failing comparison and ranking tests
Create synthetic entries whose order changes at each tie-break level. Verify one method failure does not stop the remaining methods, cancellation does, and no feasible method produces a null recommendation.
Also assert the result always contains one separately analyzed raw-path baseline plus exactly one entry for each requested method; the raw baseline is never treated as a candidate method in ranking.
- Step 2: Run and confirm RED
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_comparison.ps1
Expected: missing comparison service/ranker.
- Step 3: Implement metric normalization and lexicographic comparison
Use stable median:
double Median(IReadOnlyList<double> values)
{
double[] sorted = values.OrderBy(value => value).ToArray();
int middle = sorted.Length / 2;
return sorted.Length % 2 == 1
? sorted[middle]
: (sorted[middle - 1] + sorted[middle]) / 2d;
}
Use absolute deltas when a raw denominator has magnitude below 1e-12. The comparison service must force AllowFallbackToCoarsePath = false so fallback cannot masquerade as method success.
- Step 4: Run comparison and service tests
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_comparison.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_service.ps1
Expected: all pass.
- Step 5: Commit
git add -- ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Comparison ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingComparisonService.cs ClumsyPilot/tests/verify_path_smoothing_comparison.ps1
git commit -m "feat: compare and rank smoothing methods"
Task 10: Versioned fast fixtures and existing end-to-end scenarios
Files:
- Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Test/SmoothingScenarioFixture.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Test/SmoothingScenarioFixtureLoader.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Test/SmoothingFixtureGenerator.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Test/SmoothingScenarioFactory.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Test/Fixtures/path-smoothing-fixtures.json - Create:
ClumsyPilot/tests/generate_path_smoothing_fixtures.ps1 - Create:
ClumsyPilot/tests/verify_path_smoothing_fixtures.ps1 - Create:
ClumsyPilot/tests/verify_path_smoothing_integration.ps1
Interfaces:
-
Produces: eight immutable fast fixtures and four end-to-end comparison requests.
-
Consumes: existing
CoarsePathScenarioFactory.Create(scenario)for end-to-end requests. -
Step 1: Write failing fixture tests
Assert the exact IDs:
$expected = @(
'straight', 'single-turn', 's-bend', 'large-heading-change',
'rectangle-detour', 'multi-obstacle-detour',
'narrow-corridor', 'forward-reverse-switch')
Verify unique IDs, version > 0, matching fingerprint, valid segment coverage, and that fixture-only loading has no reference to HybridAStarPlanner.
- Step 2: Write failing end-to-end tests
For ExplicitEmpty, RectangleDetour, ManualAndTwoLeg, and ReverseGearSwitch, call the existing planner then comparison service. Assert the raw path is unchanged and every successful smoothing entry preserves segment count and gear-switch count.
- Step 3: Run and confirm RED
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_fixtures.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_integration.ps1
Expected: missing fixture loader/factory.
- Step 4: Implement deterministic JSON fixtures
Serialize with invariant numbers and explicit schema:
{
"schemaVersion": 1,
"scenarios": [
{
"id": "straight",
"fixtureVersion": 1,
"configurationFingerprint": "sha256:<lowercase-hex>",
"map": {
"xMinMm": 0, "xMaxMm": 6000,
"yMinMm": 0, "yMaxMm": 4000,
"resolutionMm": 50,
"obstacles": []
},
"vehicle": { "lengthMeters": 0.8, "widthMeters": 0.6, "safetyMarginMeters": 0.05, "maximumCurvaturePerMeter": 0.8333333333333334 },
"path": [],
"segments": []
}
]
}
Populate all eight scenarios from explicitly verified successful coarse-path outputs by running:
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\generate_path_smoothing_fixtures.ps1 -OutputPath .\ClumsyPilot\ParkrobTrajplanner\PathSmoothing\Test\Fixtures\path-smoothing-fixtures.json
SmoothingFixtureGenerator.Generate(string outputPath, bool overwrite) must refuse an existing target when overwrite is false. The script maps -Overwrite to that argument; normal tests call only LoadAndVerify and never regenerate data. Each obstacle is serialized as a typed circle or axis-aligned rectangle DTO so the loader can rebuild the exact PlanningGridMap without Hybrid A*.
- Step 5: Run fixture and integration checks
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_fixtures.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_integration.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_integration.ps1
Expected: all pass. Record actual end-to-end elapsed values as diagnostics, not hardware-dependent pass thresholds.
- Step 6: Commit
git add -- ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Test ClumsyPilot/tests/generate_path_smoothing_fixtures.ps1 ClumsyPilot/tests/verify_path_smoothing_fixtures.ps1 ClumsyPilot/tests/verify_path_smoothing_integration.ps1
git commit -m "test: add path smoothing scenarios and fixtures"
Task 11: Shared figure model, IEEE SVG, and UTF-8 CSV
Files:
- Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Visualization/IeeeFigureStyle.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Visualization/SmoothingFigureModel.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Visualization/SmoothingFigureModelBuilder.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Visualization/SmoothingSvgRenderer.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Visualization/SmoothingCsvWriter.cs - Test:
ClumsyPilot/tests/verify_path_smoothing_svg_csv.ps1
Interfaces:
-
Consumes: immutable
PathSmoothingComparisonResult, map, start/goal, scenario label. -
Produces: one immutable figure model, UTF-8 SVG, and UTF-8-BOM CSV.
-
Step 1: Write failing style and serialization tests
Assert exact colors, line styles, panel labels, view box, equal XY scale, legend order, XML escaping, UTF-8 Chinese, BOM bytes EF BB BF, and invariant decimal points.
Include one rejected candidate in the model and assert its normal method-colored curve plus violation cross markers are present while its metrics row says Infeasible. Include one numerical failure and assert its legend/row remains but no fake path element is emitted.
- Step 2: Run and confirm RED
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_svg_csv.ps1
Expected: missing IeeeFigureStyle.
- Step 3: Implement one physical layout model
Define all coordinates in typographic points (72 pt/in):
public const double FigureWidthPoints = 7.16d * 72d;
public const double FigureHeightPoints = 5.20d * 72d;
public const string RawColor = "#4D4D4D";
public const string BSplineColor = "#0072B2";
public const string BezierColor = "#D55E00";
public const string QuinticColor = "#009E73";
public const string LimitColor = "#CC79A7";
Allocate 60% width to panel (a), split the right side between (b) curvature and (c) metrics, and compute one world-to-panel transform with equal X/Y scale.
- Step 4: Implement SVG and CSV
SVG text must use explicit runs:
<text><tspan font-family="SimSun">车辆曲率 </tspan><tspan font-family="Times New Roman">κ (m⁻¹)</tspan></text>
CSV header order is fixed:
ScenarioId,Method,Status,PathLengthMeters,MaximumAbsoluteVehicleCurvaturePerMeter,RootMeanSquareVehicleCurvaturePerMeter,TotalAbsoluteCurvatureVariationPerMeter,CurvatureVariationEnergy,MinimumBodyClearanceMeters,ElapsedMilliseconds,RetryCount,AcceptedStrength
- Step 5: Run SVG/CSV and comparison checks
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_svg_csv.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_comparison.ps1
Expected: all pass.
- Step 6: Commit
git add -- ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Visualization ClumsyPilot/tests/verify_path_smoothing_svg_csv.ps1
git commit -m "feat: render ieee smoothing svg and metrics"
Task 12: Windows font validation and 300 dpi PNG export
Files:
- Modify:
ClumsyPilot/ClumsyPilot.csproj - Modify:
ClumsyPilot/ParkrobTrajplanner/Map/Test/Visualization/ValidatedPngWriter.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Visualization/SmoothingFontResolver.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Visualization/SmoothingPngRenderer.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Visualization/SmoothingReportExportRequest.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Visualization/SmoothingReportExportResult.cs - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Visualization/SmoothingReportExporter.cs - Test:
ClumsyPilot/tests/verify_path_smoothing_png.ps1
Interfaces:
-
Produces: atomic
.svg,.png,.csvexport; report failures do not mutate comparison results. -
Runtime boundary:
SmoothingPngRendereris Windows-only; SVG/CSV remain usable without GDI+. -
Step 1: Add the failing PNG/font verification
The script must assert:
PNG signature and CRC-valid chunks
IHDR width=2148 and height=1560
pHYs X=11811 and Y=11811 pixels/meter
SimSun and Times New Roman were resolved by exact family name
mixed sample "粗路径 κ(s) X (m) −π" produced non-empty glyph bounds
missing-font test returns FontUnavailable
atomic export leaves no .tmp file
- Step 2: Run and confirm RED
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_png.ps1
Expected: missing renderer and exporter.
- Step 3: Add the exact Windows rendering dependency
Add:
<PackageReference Include="System.Drawing.Common" Version="10.0.10" />
Then run:
dotnet restore .\ClumsyPilot\ClumsyPilot.csproj
Expected: restore exits 0. Do not add cross-platform fallback switches; Microsoft documents System.Drawing.Common as Windows-only.
- Step 4: Implement exact font resolution and mixed-run baseline layout
Use InstalledFontCollection to require SimSun and Times New Roman. Split text by Unicode category/CJK ranges, measure each run with typographic StringFormat, and align runs by font-family ascent:
float baseline = top + maxAscentPixels;
float runTop = baseline - family.GetCellAscent(style) * emPixels /
family.GetEmHeight(style);
Return FontUnavailable before creating output files if either exact family is absent.
- Step 5: Render the shared model and write validated PNG
Render at 2148 × 1560, opaque white background, anti-aliased geometry, and no gradients/shadows. Convert bitmap pixels to RGBA and use the existing validated PNG path, extending it to insert a pHYs chunk with 11811 pixels/meter and correct CRC before IDAT.
Export all three files to temporary siblings, validate each, then rename into place. On failure, remove only those exact temporary siblings.
- Step 6: Run PNG, map-image, and build checks
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_png.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_map_image.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_svg_csv.ps1
Expected: all pass; existing map PNG export remains valid.
- Step 7: Commit
git add -- ClumsyPilot/ClumsyPilot.csproj ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Visualization ClumsyPilot/tests/verify_path_smoothing_png.ps1
git commit -m "feat: export ieee smoothing png reports"
Task 13: Module documentation, report batch entry, and full verification
Files:
- Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/README.md - Create:
ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Test/PathSmoothingComparisonDemo.cs - Create:
ClumsyPilot/tests/run_path_smoothing_comparison.ps1 - Create:
ClumsyPilot/tests/verify_path_smoothing_documentation.ps1
Interfaces:
-
Produces: documented formal call example and a developer-only batch command that writes reports under
ClumsyPilot/obj/path_smoothing_reports. -
Step 1: Write failing documentation and batch-entry verification
Assert README contains exact sections for units, facade usage, status handling, fallback, fixture freshness, IEEE colors/fonts, Windows PNG limitation, SQP boundary, and output files. Assert the batch runner offers -FixtureOnly and never writes under source directories.
- Step 2: Run and confirm RED
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_documentation.ps1
Expected: missing README and demo.
- Step 3: Write README and batch demo
Document this minimal formal flow:
if (coarseResult.PlanningResult.Status != PlanningStatus.Success)
return;
var smoothing = new PathSmoothingService().Smooth(
new PathSmoothingRequest(
coarseResult.PlanningResult.Path,
coarseResult.PlanningResult.Segments,
coarseResult.MapResult.Map,
job.Vehicle,
smoothingConfiguration),
cancellationToken);
if (smoothing.Status == PathSmoothingStatus.Success ||
smoothing.Status == PathSmoothingStatus.FallbackToCoarsePath)
ConsumeSpatialReference(smoothing.Path, smoothing.Segments);
The batch script builds once, runs eight fixtures by default, optionally runs the four end-to-end cases, prints one-line metrics per method, and writes only beneath ClumsyPilot/obj/path_smoothing_reports.
- Step 4: Run the complete verification suite
dotnet build .\ClumsyPilot\ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_utils.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_map_adapter.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_map_factory.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_planning_map_image.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_collision.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_search.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_coarse_path_integration.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_contracts.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_geometry.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_validation.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_runner.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_bspline.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_bezier.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_quintic.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_service.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_comparison.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_fixtures.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_integration.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_svg_csv.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_png.ps1
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\verify_path_smoothing_documentation.ps1
Expected: build exits 0 and every script prints its passed message with no terminating errors.
- Step 5: Generate and inspect representative reports
powershell -ExecutionPolicy Bypass -File .\ClumsyPilot\tests\run_path_smoothing_comparison.ps1 -FixtureOnly
Inspect straight, rectangle-detour, and forward-reverse-switch PNG/SVG files. Confirm four-method legend order, equal path axes, readable 8–10 pt text, no Chinese mojibake, no clipping/overlap, curvature limits, and infeasible markers where applicable.
- Step 6: Commit
git add -- ClumsyPilot/ParkrobTrajplanner/PathSmoothing/README.md ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Test/PathSmoothingComparisonDemo.cs ClumsyPilot/tests/run_path_smoothing_comparison.ps1 ClumsyPilot/tests/verify_path_smoothing_documentation.ps1
git commit -m "docs: document path smoothing comparison workflow"