First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Files
ddea58a1397261bdbaff5ed886a3aa978a5f97e0
ParkingRobot/docs/superpowers/plans/2026-07-30-local-g2-path-presmoothing.md
T

62 KiB
Raw Blame History

Local G2 Path Presmoothing 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: 在 Hybrid A* 与 SQP 之间新增局部 G2 五次 Hermite 预平滑主路线，安全地改善可处理的运动基元曲率跳变，并输出完整、可诊断的 SQP 初始路径。

Architecture: 新路线作为 PathSmoothingService 中的专用分支实现，不塞入旧的“强度重试”运行器。LocalG2 子目录负责曲率事件检测、窗口规划、五次 Hermite 几何、候选构造与评价；现有 PathGeometryAnalyzer 和 SmoothedPathValidator 继续作为统一几何和完整车体安全真源。

Tech Stack: C# 10、.NET Standard 2.0、现有规划栅格和完整车体碰撞检查器、PowerShell 反射验证脚本、Git。

Design spec: docs/superpowers/specs/2026-07-30-local-g2-path-presmoothing-design.md

Global Constraints

  • 不修改 Hybrid A* 的几何中心参考模型、搜索代价或运动基元生成策略。
  • 不实现 SQP、速度规划、轮速分解或每点速度上限。
  • 前进/倒车是硬分段；绝不跨换向点计算或平滑曲率。
  • 单事件窗口默认 0.20 m、0.50 m、0.80 m，允许左右不对称。
  • 最大局部路径偏移默认 0.10 m。
  • 曲率事件检测门槛为 max(0.001 1/m, 0.05 × 最大车辆曲率)。
  • 成功区域的峰值 |dκ/ds| 至少改善 20%。
  • 曲率变化代价只允许 2% 数值容差。
  • 每个区域最多评估 12 个确定性候选。
  • 输出采样间距和连续碰撞检查步长默认均为 0.025 m。
  • 扩大车体之外仍需保留默认 0.02 m 净空。
  • 旧 B 样条、局部 Bézier和旧五次算法不重写；其现有测试必须保持通过。
  • 所有新行为先写失败测试，再写最小实现；每个任务单独提交。
  • 工作区已有未提交修改。每次暂存必须显式列出本任务文件，禁止覆盖或顺带提交无关改动。

File Structure

Public contracts

  • Create: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/LocalG2QuinticOptions.cs 新主路线的语义化配置和默认值。
  • Create: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRegionStatus.cs 单区域“已改善/保留原路径”状态。
  • Create: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRegionFailureReason.cs 稳定的区域失败原因枚举。
  • Create: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRegionReport.cs 不可变区域报告。
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothingMethod.cs 追加 LocalG2Quintic，不改变旧成员顺序。
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingStatus.cs 追加 Complete、PartialImprovement、NotNeeded、Unchanged。
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathPointSource.cs 追加 LocalG2Transition。
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathPoint.cs 新增 VehicleCurvatureDerivative，保留旧构造函数兼容性。
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathQualityMetrics.cs 新增峰值曲率变化率和“曲率变化代价”别名。
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingResult.cs 新增区域报告和本地主路线发布工厂。
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingConfiguration.cs 暴露 LocalG2Quintic 配置。
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRequest.cs 深复制新配置。

Local G2 implementation

  • Create: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2OptionsSnapshot.cs 已校验的不可变配置快照。
  • Create: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/CurvatureTransition.cs 单个同方向运动基元曲率事件。
  • Create: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/CurvatureTransitionDetector.cs 从原始粗路径检测事件。
  • Create: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2SmoothingRegion.cs 一个独立或合并后的局部区域。
  • Create: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2WindowPlanner.cs 生成长度和左右分配均有界的区域候选。
  • Create: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/QuinticHermiteCurve2D.cs 纯二维五次 Hermite 数学。
  • Create: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateGeometry.cs 一个待验证的局部替换几何。
  • Create: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs 用方向、端点曲率和内部共享曲率生成有限候选。
  • Create: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2PathSplicer.cs 将局部替换拼入完整 PreparedPath。
  • Create: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateEvaluator.cs 统一分析、偏移、质量、碰撞、净空和确定性排序。
  • Create: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2PreSmoothingPipeline.cs 按区域运行、局部回退、全局复验并生成最终状态和报告。

Shared infrastructure

  • Modify: ClumsyPilot/ParkrobTrajplanner/CoarsePath/Output/CoarsePathAssembler.cs 保留真实起点车辆曲率。
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalysis.cs 暴露峰值 |dκ/ds|。
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalyzer.cs 在方向段内统一计算每点 dκ/ds 和峰值。
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PreparedDirectionSegment.cs 可选保留方向段起点的真实车辆曲率边界条件。
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathSmoothingPreprocessor.cs 将粗路径首点曲率传入方向段边界条件。
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/RawPathBaselineBuilder.cs 粗路径也使用统一几何分析器。
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Validation/SmoothedPathValidator.cs 校验并保留每点 dκ/ds。
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingService.cs 验证新配置并分派至专用流水线。
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingComparisonService.cs 适配统一粗路径基线签名；默认比较列表仍只有旧三种算法。
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/README.md 记录新主路线、状态和 SQP 边界。

Verification

  • Modify: ClumsyPilot/tests/verify_coarse_path_integration.ps1
  • Modify: ClumsyPilot/tests/verify_path_smoothing_contracts.ps1
  • Modify: ClumsyPilot/tests/verify_path_smoothing_geometry.ps1
  • Create: ClumsyPilot/tests/verify_path_smoothing_local_g2_detection.ps1
  • Create: ClumsyPilot/tests/verify_path_smoothing_local_g2_curve.ps1
  • Create: ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1
  • Create: ClumsyPilot/tests/verify_path_smoothing_local_g2_integration.ps1
  • Modify: ClumsyPilot/tests/verify_path_smoothing_documentation.ps1

Task 1: Add Local G2 public contracts and immutable configuration

Files:

  • Create: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/LocalG2QuinticOptions.cs
  • Create: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRegionStatus.cs
  • Create: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRegionFailureReason.cs
  • Create: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRegionReport.cs
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothingMethod.cs
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingStatus.cs
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathPointSource.cs
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathPoint.cs
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathQualityMetrics.cs
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingResult.cs
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingConfiguration.cs
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRequest.cs
  • Test: ClumsyPilot/tests/verify_path_smoothing_contracts.ps1

Interfaces:

  • Produces: SmoothingMethod.LocalG2Quintic.

  • Produces: PathSmoothingStatus.Complete, PartialImprovement, NotNeeded, Unchanged.

  • Produces: LocalG2QuinticOptions with the exact defaults from Global Constraints.

  • Produces: SmoothedPathPoint.VehicleCurvatureDerivative.

  • Produces: PathSmoothingResult.PublishLocalG2(...).

  • Produces: immutable PathSmoothingResult.RegionReports.

  • Step 1: Extend the contract verification first

Append assertions equivalent to:

$localOptionsType = Get-RequiredType ($root + 'LocalG2QuinticOptions')
$regionStatusType = Get-RequiredType ($root + 'PathSmoothingRegionStatus')
$regionFailureType = Get-RequiredType ($root + 'PathSmoothingRegionFailureReason')
$regionReportType = Get-RequiredType ($root + 'PathSmoothingRegionReport')

Assert-Equal 'CubicBSpline,LocalCubicBezier,PiecewiseQuintic,LocalG2Quintic' `
    ([string]::Join(',', [Enum]::GetNames($methodType))) `
    'The Local G2 method must be appended without reordering legacy methods.'
Assert-True ([Enum]::GetNames($statusType) -contains 'Complete') 'Complete status is required.'
Assert-True ([Enum]::GetNames($statusType) -contains 'PartialImprovement') 'Partial status is required.'
Assert-True ([Enum]::GetNames($statusType) -contains 'NotNeeded') 'NotNeeded status is required.'
Assert-True ([Enum]::GetNames($statusType) -contains 'Unchanged') 'Unchanged status is required.'

$local = [Activator]::CreateInstance($localOptionsType)
Assert-Near 0.20 $local.MinimumWindowLengthMeters 'Minimum Local G2 window must be 0.20 m.'
Assert-Near 0.50 $local.PreferredWindowLengthMeters 'Preferred Local G2 window must be 0.50 m.'
Assert-Near 0.80 $local.MaximumWindowLengthMeters 'Maximum Local G2 window must be 0.80 m.'
Assert-Near 0.10 $local.MaximumDeviationMeters 'Maximum Local G2 deviation must be 0.10 m.'
Assert-Near 0.001 $local.AbsoluteCurvatureJumpFloorPerMeter 'Absolute jump floor must be 0.001 1/m.'
Assert-Near 0.05 $local.CurvatureJumpRatioOfMaximum 'Relative jump threshold must be 5 percent.'
Assert-Near 0.20 $local.MinimumPeakGradientImprovementRatio 'Peak improvement must be 20 percent.'
Assert-Near 0.02 $local.MaximumVariationCostRegressionRatio 'Variation cost tolerance must be 2 percent.'
Assert-Equal 12 $local.MaximumCandidatesPerRegion 'At most twelve candidates are allowed.'

Assert-True ($pointType.GetProperty('VehicleCurvatureDerivative') -ne $null) `
    'Smoothed points must expose d-kappa/d-s.'
Assert-True ($resultType.GetProperty('RegionReports') -ne $null) `
    'Local G2 results must expose immutable region reports.'

Also mutate every new option on the original configuration after building a PathSmoothingRequest, then assert the request snapshot still contains defaults. Repeat through the configuration getter to prove defensive copying.

  • Step 2: Run the unchanged assembly against the new contract test

Run:

dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_contracts.ps1

Expected: build passes; contract script fails because LocalG2QuinticOptions or the new enum members do not exist.

  • Step 3: Implement the public options and enum additions

Create the options exactly as:

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing;

public sealed class LocalG2QuinticOptions
{
    public double MinimumWindowLengthMeters { get; set; } = 0.20d;
    public double PreferredWindowLengthMeters { get; set; } = 0.50d;
    public double MaximumWindowLengthMeters { get; set; } = 0.80d;
    public double MaximumDeviationMeters { get; set; } = 0.10d;
    public double AbsoluteCurvatureJumpFloorPerMeter { get; set; } = 0.001d;
    public double CurvatureJumpRatioOfMaximum { get; set; } = 0.05d;
    public double MinimumPeakGradientImprovementRatio { get; set; } = 0.20d;
    public double MaximumVariationCostRegressionRatio { get; set; } = 0.02d;
    public int MaximumCandidatesPerRegion { get; set; } = 12;
}

Append, rather than reorder, enum members:

public enum PathSmoothingRegionStatus
{
    Improved,
    RetainedOriginal,
}

public enum PathSmoothingRegionFailureReason
{
    None,
    WindowUnavailable,
    CandidateGenerationFailed,
    Collision,
    InsufficientClearance,
    CurvatureExceeded,
    CurvatureOvershoot,
    DeviationExceeded,
    InsufficientImprovement,
    VariationCostRegression,
    GlobalValidationRollback,
}

Add LocalG2Transition at the end of SmoothedPathPointSource. Add a new SmoothedPathPoint constructor containing vehicleCurvatureDerivativePerSquareMeter immediately after vehicle curvature; retain the old constructor and delegate to the new one with 0d.

  • Step 4: Implement immutable report and result publication

Use this exact public report constructor:

public PathSmoothingRegionReport(
    int segmentIndex,
    double startArcLengthMeters,
    double endArcLengthMeters,
    IReadOnlyList<double> curvatureJumpsPerMeter,
    double plannedWindowLengthMeters,
    double actualWindowLengthMeters,
    double leftWindowLengthMeters,
    double rightWindowLengthMeters,
    int candidateCount,
    int selectedCandidateIndex,
    PathSmoothingRegionStatus status,
    PathSmoothingRegionFailureReason failureReason,
    double rawPeakCurvatureDerivativePerSquareMeter,
    double resultPeakCurvatureDerivativePerSquareMeter,
    double rawCurvatureVariationCost,
    double resultCurvatureVariationCost,
    double maximumDeviationMeters,
    double minimumBodyClearanceMeters,
    double maximumAbsoluteVehicleCurvaturePerMeter)

Use this factory boundary:

public static PathSmoothingResult PublishLocalG2(
    PathSmoothingStatus status,
    IReadOnlyList<SmoothedPathPoint> path,
    IReadOnlyList<SmoothedPathSegment> segments,
    PathSmoothingDiagnostics diagnostics,
    IReadOnlyList<PathSmoothingRegionReport> regionReports)

Allow only Complete, PartialImprovement, NotNeeded, and Unchanged; require feasible diagnostics and a non-empty path. Failure continues to publish an empty path. Existing Success and Fallback factories remain source-compatible and publish an empty immutable report collection.

PathSmoothingRegionReport must defensively copy its curvature-jump list. Use SelectedCandidateIndex = -1 when no candidate was selected.

Add MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter to PathQualityMetrics. Retain the old constructor and delegate to a new constructor that accepts the peak derivative immediately after maximum absolute curvature. Keep:

public double CurvatureVariationCost => CurvatureVariationEnergy;

as a compatibility-safe name for new code and reports.

  • Step 5: Copy and validate the new configuration

Add:

public LocalG2QuinticOptions LocalG2Quintic { get; } = new LocalG2QuinticOptions();

to PathSmoothingConfiguration. Copy every new property in PathSmoothingRequest.CopyConfiguration; do not reuse the caller-owned options object.

  • Step 6: Build and run the contract test

Run:

dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_contracts.ps1

Expected: build succeeds and output ends with Path smoothing contract checks passed.

  • Step 7: Commit only Task 1 files
git add -- `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/LocalG2QuinticOptions.cs `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRegionStatus.cs `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRegionFailureReason.cs `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRegionReport.cs `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothingMethod.cs `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingStatus.cs `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathPointSource.cs `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/SmoothedPathPoint.cs `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathQualityMetrics.cs `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingResult.cs `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingConfiguration.cs `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Contracts/PathSmoothingRequest.cs `
  ClumsyPilot/tests/verify_path_smoothing_contracts.ps1
git diff --cached --check
git commit -m "feat: add local G2 smoothing contracts"

Task 2: Preserve the true Hybrid A* start curvature

Files:

  • Modify: ClumsyPilot/ParkrobTrajplanner/CoarsePath/Output/CoarsePathAssembler.cs
  • Modify: ClumsyPilot/tests/verify_coarse_path_integration.ps1

Interfaces:

  • Consumes: PlanningRequest.StartVehicleCurvature.

  • Produces: PlanningResult.Path[0].VehicleCurvature == StartVehicleCurvature.

  • Later tasks rely on the first point to expose a real start-to-first-primitive transition.

  • Step 1: Add a failing end-to-end assertion

Before the first successful Plan call in verify_coarse_path_integration.ps1, set:

$request.StartVehicleCurvature = [double]0.20

After success, add:

Assert-Near 0.20 $result.Path[0].VehicleCurvature `
    'The assembled start point must retain the requested physical vehicle curvature.'
  • Step 2: Run the regression test and verify RED

Run:

dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_coarse_path_integration.ps1

Expected: fail at the new start-curvature assertion because the assembler currently publishes the first primitive curvature at index zero.

  • Step 3: Make the minimal assembler change

Replace:

double currentCurvature = backtrackedPath.Primitives.Count > 0
    ? backtrackedPath.Primitives[0].CurvaturePerMeter
    : backtrackedPath.StartCurvaturePerMeter;

with:

double currentCurvature = backtrackedPath.StartCurvaturePerMeter;

Do not change currentDirection: direction still comes from the first primitive when one exists.

  • Step 4: Run CoarsePath regression checks

Run:

dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_coarse_path_integration.ps1
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_coarse_path_search.ps1

Expected: both scripts end in their ... checks passed. messages.

  • Step 5: Commit
git add -- `
  ClumsyPilot/ParkrobTrajplanner/CoarsePath/Output/CoarsePathAssembler.cs `
  ClumsyPilot/tests/verify_coarse_path_integration.ps1
git diff --cached --check
git commit -m "fix: preserve coarse path start curvature"

Task 3: Compute dκ/ds once and use one fair raw-path analyzer

Files:

  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalysis.cs
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalyzer.cs
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PreparedDirectionSegment.cs
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathSmoothingPreprocessor.cs
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/RawPathBaselineBuilder.cs
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Validation/SmoothedPathValidator.cs
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingService.cs
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingComparisonService.cs
  • Modify: ClumsyPilot/tests/verify_path_smoothing_geometry.ps1
  • Modify: ClumsyPilot/tests/verify_path_smoothing_validation.ps1

Interfaces:

  • Produces: PathGeometryAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter.

  • Produces: each analyzed SmoothedPathPoint.VehicleCurvatureDerivative.

  • Produces: optional PreparedDirectionSegment.StartVehicleCurvaturePerMeter, used only when a real boundary state is available.

  • Produces: RawPathBaselineBuilder.TryCreate(..., PathGeometryAnalyzer analyzer, double outputSpacingMeters, ...).

  • Guarantees: no derivative crosses a gear-switch duplicate.

  • Step 1: Add failing geometry assertions

Extend verify_path_smoothing_geometry.ps1:

Assert-Near 0.0 $straightAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter 0.000000001 `
    'A straight must have zero peak d-kappa/d-s.'
foreach ($point in $straightAnalysis.Path) {
    Assert-Near 0.0 $point.VehicleCurvatureDerivative 0.000000001 `
        'A straight point must carry zero d-kappa/d-s.'
}

Assert-True (-not [double]::IsNaN($switchLeft.VehicleCurvatureDerivative)) `
    'The forward side of a gear switch must have a finite one-sided derivative.'
Assert-True (-not [double]::IsNaN($switchRight.VehicleCurvatureDerivative)) `
    'The reverse side of a gear switch must have a finite one-sided derivative.'
Assert-Near 0.0 $reverseStraightStart.VehicleCurvatureDerivative 0.000000001 `
    'No curvature derivative may cross from the preceding forward arc into a reverse straight.'

$physicalStart = New-DirectionSegmentWithStartCurvature 0 $forward 0.20 @(
    (New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
    (New-GeometryPoint 1.0 0.0 1.0 0.0 0.0))
$physicalStartAnalysis = Invoke-Analysis @($physicalStart)
Assert-Near 0.20 $physicalStartAnalysis.Path[0].VehicleCurvature 0.000000001 `
    'The unified analyzer must retain a real start steering-curvature boundary state.'
Assert-True ($physicalStartAnalysis.MaximumAbsoluteVehicleCurvatureDerivativePerSquareMeter -gt 0.0) `
    'A real start-curvature mismatch must remain visible to the quality analyzer.'

Extend validation tests to assert that a nonzero derivative survives clearance recomputation unchanged.

  • Step 2: Run tests and verify RED
dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_geometry.ps1

Expected: fail because analysis and points do not expose the derivative.

  • Step 3: Preserve real direction-segment start curvature

Add a constructor overload:

public PreparedDirectionSegment(
    int segmentIndex,
    TravelDirection direction,
    IReadOnlyList<SmoothingPoint2D> points,
    bool startsAtGearSwitch,
    bool endsAtGearSwitch,
    double? startVehicleCurvaturePerMeter)

The existing five-argument constructor delegates with null, preserving legacy algorithms. Validate a supplied value as finite and expose it as double? StartVehicleCurvaturePerMeter.

PathSmoothingPreprocessor must call the new overload with the original CoarsePathPoint.VehicleCurvature at that direction segment’s first raw index. Local G2 splicing in Task 6 must carry the value forward unchanged.

  • Step 4: Compute curvature derivatives per direction segment

In TryAnalyzeSegment, first compute all vehicleCurvatures, then compute:

if (count == 1)
{
    curvatureDerivatives[index] = 0d;
}
else if (index == 0)
{
    curvatureDerivatives[index] =
        (vehicleCurvatures[1] - vehicleCurvatures[0]) /
        (localArcLengths[1] - localArcLengths[0]);
}
else if (index == count - 1)
{
    curvatureDerivatives[index] =
        (vehicleCurvatures[index] - vehicleCurvatures[index - 1]) /
        (localArcLengths[index] - localArcLengths[index - 1]);
}
else
{
    curvatureDerivatives[index] =
        (vehicleCurvatures[index + 1] - vehicleCurvatures[index - 1]) /
        (localArcLengths[index + 1] - localArcLengths[index - 1]);
}

Reject non-finite derivatives. Track the maximum absolute derivative. Construct output points only after both arrays are complete. Keep all calculations inside the current direction segment loop.

After ordinary geometric curvature is computed, but before dκ/ds, apply a finite start-boundary override:

if (segment.StartVehicleCurvaturePerMeter.HasValue)
{
    double directionSign = segment.Direction == TravelDirection.Forward ? 1d : -1d;
    vehicleCurvatures[0] = segment.StartVehicleCurvaturePerMeter.Value;
    geometricCurvatures[0] = directionSign * vehicleCurvatures[0];
}

This is required because the vehicle’s physical start steering state is not recoverable from the first outgoing chord. Both raw and candidate paths pass through this same analyzer and the same boundary override.

Rename only the user-facing description from “energy” to “cost”; retain the existing CurvatureVariationEnergy property as a compatibility alias:

public double CurvatureVariationCost => CurvatureVariationEnergy;
  • Step 5: Replace the unfair raw baseline

Change RawPathBaselineBuilder.TryCreate to analyze preparedPath.Segments through the same PathGeometryAnalyzer and outputSpacingMeters, then validate that analyzed path:

if (!analyzer.TryAnalyze(
        preparedPath.Segments,
        outputSpacingMeters,
        out PathGeometryAnalysis analysis,
        out reason))
{
    return false;
}

if (!validator.TryValidate(
        analysis.Path,
        analysis.Segments,
        preparedPath,
        request.Map,
        request.Vehicle,
        maximumCollisionCheckStepMeters,
        out IReadOnlyList<SmoothedPathPoint> safePath,
        out double minimumClearanceMeters,
        out reason))
{
    return false;
}

Build baseline metrics from analysis, including peak derivative, instead of recomputing from stored Hybrid A* curvature. Update all three callers with one shared analyzer and configuration.OutputSpacingMeters.

  • Step 6: Preserve derivatives through validation

Update SmoothedPathValidator.IsValidPoint to require a finite derivative. When rebuilding points with checked clearance, copy VehicleCurvatureDerivative rather than falling back to the legacy constructor.

  • Step 7: Run focused and legacy tests
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_validation.ps1
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_service.ps1
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_comparison.ps1

Expected: all four scripts pass. If old comparison percentages change because the baseline is now fair, update assertions to the newly unified calculations, not to hard-coded legacy stored-curvature values.

  • Step 8: Commit
git add -- `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalysis.cs `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalyzer.cs `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PreparedDirectionSegment.cs `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathSmoothingPreprocessor.cs `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/RawPathBaselineBuilder.cs `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Validation/SmoothedPathValidator.cs `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingService.cs `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingComparisonService.cs `
  ClumsyPilot/tests/verify_path_smoothing_geometry.ps1 `
  ClumsyPilot/tests/verify_path_smoothing_validation.ps1
git diff --cached --check
git commit -m "fix: unify path smoothing geometry metrics"

Task 4: Detect curvature events and plan bounded merged windows

Files:

  • Create: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2OptionsSnapshot.cs
  • Create: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/CurvatureTransition.cs
  • Create: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/CurvatureTransitionDetector.cs
  • Create: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2SmoothingRegion.cs
  • Create: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2WindowPlanner.cs
  • Create: ClumsyPilot/tests/verify_path_smoothing_local_g2_detection.ps1

Interfaces:

  • Produces:
internal bool CurvatureTransitionDetector.TryDetect(
    PathSmoothingRequest request,
    double maximumVehicleCurvaturePerMeter,
    LocalG2OptionsSnapshot options,
    out IReadOnlyList<CurvatureTransition> transitions,
    out string reason);
  • Produces:
internal bool LocalG2WindowPlanner.TryPlan(
    PreparedPath originalPath,
    IReadOnlyList<CurvatureTransition> transitions,
    LocalG2OptionsSnapshot options,
    out IReadOnlyList<LocalG2SmoothingRegion> regions,
    out string reason);
  • CurvatureTransition exposes these immutable values:
int SegmentIndex
int LeftCoarsePathIndex
int RightCoarsePathIndex
double LocalArcLengthMeters
double X
double Y
double VehicleHeadingRadians
double LeftVehicleCurvaturePerMeter
double RightVehicleCurvaturePerMeter
double CurvatureJumpPerMeter

  • Define LocalG2WindowVariant in the region file with CandidateIndex, StartArcLengthMeters, EndArcLengthMeters, LeftWindowLengthMeters, and RightWindowLengthMeters. LocalG2SmoothingRegion exposes SegmentIndex, ordered Transitions, the maximum merged range, and ordered IReadOnlyList<LocalG2WindowVariant> WindowVariants.

  • Guarantees: transition arc length is the left/ending point of the old primitive, not the first sampled point inside the new primitive.

  • Step 1: Write detection and window tests

The new reflection script must cover these scenarios:

# Same direction: 0 -> 0.4167 is detected once.
Assert-Equal 1 $singleTransition.TransitionCount 'One primitive curvature jump must be detected.'
Assert-Near 0.4167 $singleTransition.MaximumJump 0.0001 'The jump magnitude must be retained.'

# Same pose at a forward/reverse boundary: no event crosses the stop.
Assert-Equal 0 $gearSwitch.TransitionCount 'A stopped gear switch must not be a smoothing event.'

# A 0.01 1/m numerical change is below max(0.001, 5% of 0.8333).
Assert-Equal 0 $noise.TransitionCount 'Sub-threshold curvature noise must be ignored.'

# Two 0.50 m windows whose ranges overlap are merged.
Assert-Equal 1 $overlap.RegionCount 'Overlapping windows must form one joint region.'
Assert-Equal 2 $overlap.TransitionCountInFirstRegion 'The merged region must retain both events.'

# Near a segment start, the window becomes asymmetric without crossing the hard boundary.
Assert-Near 0.0 $nearStart.StartArcLength 0.000000001 'A start window must be clamped to the segment.'
Assert-True ($nearStart.RightWindowLength -gt $nearStart.LeftWindowLength) `
    'Unavailable left length must be shifted to the right.'

Expose a deterministic public static TestHooks.Execute(string scenario) snapshot on CurvatureTransitionDetector so the script does not need to instantiate internal event types directly.

  • Step 2: Run and verify RED
dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_local_g2_detection.ps1

Expected: fail because the Local G2 detector does not exist.

  • Step 3: Implement strict option validation

LocalG2OptionsSnapshot must reject:

minimum <= 0
preferred < minimum
maximum < preferred
maximumDeviation <= 0
absoluteJumpFloor <= 0
jumpRatio <= 0 || jumpRatio > 1
minimumImprovement <= 0 || minimumImprovement >= 1
costRegression < 0
maximumCandidates < 1

Copy scalar values into readonly properties. Do not retain LocalG2QuinticOptions.

  • Step 4: Implement event detection

For each PathSegment, compare adjacent raw points only within its inclusive index range:

double threshold = Math.Max(
    options.AbsoluteCurvatureJumpFloorPerMeter,
    options.CurvatureJumpRatioOfMaximum * maximumVehicleCurvaturePerMeter);

double delta = right.VehicleCurvature - left.VehicleCurvature;
if (Math.Abs(delta) >= threshold)
{
    double segmentStartArc = coarsePath[segment.StartIndex].ArcLength;
    transitions.Add(new CurvatureTransition(
        segment.SegmentIndex,
        leftIndex,
        rightIndex,
        left.ArcLength - segmentStartArc,
        left.X,
        left.Y,
        left.Heading,
        left.VehicleCurvature,
        right.VehicleCurvature));
}

Never compare segment.EndIndex with the next segment start. Validate finite values and nondecreasing arc length.

  • Step 5: Implement window variants and merging

Generate deterministic total-length targets in this order, removing duplicates after clamping:

preferred
0.75 × preferred
1.25 × preferred
minimum
maximum

For each target, start with a 50/50 split. If one side reaches a hard boundary, shift the missing length to the other side without exceeding the target or segment length. Also generate 40/60 and 60/40 variants when both are legal. Stop after MaximumCandidatesPerRegion.

For region discovery, use each event's maximum legal window so that no later candidate can overlap a neighboring region that was treated independently. Sort by segment and start arc; merge ranges that overlap or touch within 1e-9 m. A merged LocalG2SmoothingRegion retains all ordered events and all legal window variants.

  • Step 6: Run tests
dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_local_g2_detection.ps1
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_geometry.ps1

Expected: both scripts pass.

  • Step 7: Commit
git add -- `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2OptionsSnapshot.cs `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/CurvatureTransition.cs `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/CurvatureTransitionDetector.cs `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2SmoothingRegion.cs `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2WindowPlanner.cs `
  ClumsyPilot/tests/verify_path_smoothing_local_g2_detection.ps1
git diff --cached --check
git commit -m "feat: detect local curvature transition regions"

Task 5: Implement the pure quintic Hermite geometry

Files:

  • Create: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/QuinticHermiteCurve2D.cs
  • Create: ClumsyPilot/tests/verify_path_smoothing_local_g2_curve.ps1

Interfaces:

  • Produces:
internal static bool QuinticHermiteCurve2D.TryCreate(
    double x0, double y0, double dx0, double dy0, double ddx0, double ddy0,
    double x1, double y1, double dx1, double dy1, double ddx1, double ddy1,
    out QuinticHermiteCurve2D curve,
    out string reason);

internal void Evaluate(
    double u,
    out double x, out double y,
    out double dx, out double dy,
    out double ddx, out double ddy);

  • Guarantees: exact endpoint position, first derivative and second derivative within 1e-10.

  • Step 1: Write the math tests

Add scenarios for a straight and a curved boundary:

$straight = Invoke-Curve 0 0 1 0 0 0  2 0 1 0 0 0
Assert-Near 0.0 $straight.Start.X 1e-12 'Start X must match.'
Assert-Near 2.0 $straight.End.X 1e-12 'End X must match.'
Assert-Near 0.0 $straight.Mid.Y 1e-12 'A straight Hermite curve must remain on the axis.'

$curved = Invoke-Curve 0 0 1 0 0 0.4  1 1 0 1 -0.4 0
Assert-Near 0.4 $curved.Start.Curvature 1e-10 'Start curvature must match boundary derivatives.'
Assert-Near 0.4 $curved.End.Curvature 1e-10 'End curvature must match boundary derivatives.'
Assert-True $curved.AllFinite 'All sampled values must be finite.'

Assert-Rejected (Invoke-InvalidCurve 'ZeroFirstDerivative') `
    'A curve endpoint with zero first derivative must be rejected.'

Use curvature:

(dx * ddy - dy * ddx) / (dx² + dy²)^(3/2)
  • Step 2: Run and verify RED
dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_local_g2_curve.ps1

Expected: fail because QuinticHermiteCurve2D is absent.

  • Step 3: Implement the coefficient solver

For each coordinate, compute:

a0 = p0;
a1 = v0;
a2 = acceleration0 / 2d;
double c0 = p1 - (a0 + a1 + a2);
double c1 = v1 - (a1 + 2d * a2);
double c2 = acceleration1 - 2d * a2;
a3 = 10d * c0 - 4d * c1 + 0.5d * c2;
a4 = -15d * c0 + 7d * c1 - c2;
a5 = 6d * c0 - 3d * c1 + 0.5d * c2;

Evaluate with Horner form. Validate u ∈ [0,1], finite inputs and nonzero endpoint derivative norms. The math class must not know about maps, vehicles, SQP or public path contracts.

  • Step 4: Run tests
dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_local_g2_curve.ps1

Expected: Local G2 quintic curve checks passed.

  • Step 5: Commit
git add -- `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/QuinticHermiteCurve2D.cs `
  ClumsyPilot/tests/verify_path_smoothing_local_g2_curve.ps1
git diff --cached --check
git commit -m "feat: add quintic Hermite curve geometry"

Task 6: Build and splice deterministic Local G2 candidates

Files:

  • Create: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateGeometry.cs
  • Create: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs
  • Create: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2PathSplicer.cs
  • Create: ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1

Interfaces:

  • Produces:
internal IReadOnlyList<LocalG2CandidateGeometry> Build(
    PreparedDirectionSegment originalSegment,
    LocalG2SmoothingRegion region,
    double outputSpacingMeters,
    LocalG2OptionsSnapshot options,
    CancellationToken cancellationToken);
  • Produces:
internal bool LocalG2PathSplicer.TryReplace(
    PreparedPath currentPath,
    LocalG2CandidateGeometry candidate,
    out PreparedPath replacedPath,
    out string reason);
  • LocalG2CandidateGeometry exposes:
int CandidateIndex
int SegmentIndex
double StartArcLengthMeters
double EndArcLengthMeters
double LeftWindowLengthMeters
double RightWindowLengthMeters
IReadOnlyList<SmoothingPoint2D> RegionPoints

  • Guarantees: candidate endpoints exactly match original window reference; no duplicate non-gear points are introduced.

  • Step 1: Add failing candidate tests

The script must assert:

Assert-True ($isolated.CandidateCount -gt 0 -and $isolated.CandidateCount -le 12) `
    'An isolated event must produce a bounded non-empty candidate set.'
Assert-Near 0.0 $isolated.StartPositionError 1e-9 'Window start position must match.'
Assert-Near 0.0 $isolated.EndPositionError 1e-9 'Window end position must match.'
Assert-Near 0.0 $isolated.StartCurvatureError 1e-8 'Window start curvature must match.'
Assert-Near 0.0 $isolated.EndCurvatureError 1e-8 'Window end curvature must match.'
Assert-True $isolated.ContainsLocalG2Source 'Generated samples must identify their source.'

Assert-Equal 1 $cluster.OutputRegionCount 'Overlapping events must be built as one region.'
Assert-True $cluster.InternalConnectionsAreG2 'Internal anchors must share tangent and curvature.'

Assert-Equal 'Reverse' $reverse.Direction 'Reverse candidates must preserve segment direction.'
Assert-True $reverse.VehicleAndGeometricCurvatureSignsAreOpposite `
    'Reverse candidates must convert vehicle curvature to geometric curvature exactly once.'

Assert-True $spliced.NoDuplicateNonGearPoints 'Splicing must remove duplicate boundary samples.'
Assert-True $spliced.EndpointsUnchanged 'Splicing must preserve the complete segment endpoints.'
  • Step 2: Run and verify RED
dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1

Expected: fail because candidate construction is absent.

  • Step 3: Construct Hermite boundary derivatives

For direction sign:

double directionSign = segment.Direction == TravelDirection.Forward ? 1d : -1d;
double travelHeading = segment.Direction == TravelDirection.Forward
    ? vehicleHeading
    : vehicleHeading - Math.PI;
double geometricCurvature = directionSign * vehicleCurvature;
double tx = Math.Cos(travelHeading);
double ty = Math.Sin(travelHeading);
double nx = -ty;
double ny = tx;
double dx = derivativeScale * tx;
double dy = derivativeScale * ty;
double ddx = derivativeScale * derivativeScale * geometricCurvature * nx;
double ddy = derivativeScale * derivativeScale * geometricCurvature * ny;

Use one shared position, heading, derivative scale and shared curvature at every internal primitive boundary. The initial shared curvature is the distance-weighted value from the design spec. Sample each curve densely enough that adjacent raw candidate points are no farther than outputSpacingMeters.

If the first event lies at the direction-segment start, do not create a zero-length left Hermite piece. Use PreparedDirectionSegment.StartVehicleCurvaturePerMeter as the outer start curvature and construct the one-sided transition directly toward the first stable primitive state.

  • Step 4: Generate finite candidate combinations

Use window variants from Task 4. For each window, use derivative-scale multipliers in fixed order:

1.00
0.85
1.15

The base scale for a curve piece is that piece’s original arc length. Stop at MaximumCandidatesPerRegion. Reject a candidate immediately if any curve evaluation is non-finite or its derivative norm falls below 1e-10.

  • Step 5: Implement safe splicing

Interpolate exact window endpoints using PathReferenceInterpolator.TryInterpolateByArcLength. Copy original points strictly before the start, append candidate region points, then copy original points strictly after the end. Recalculate local prepared arc lengths monotonically from geometry. Preserve StartsAtGearSwitch and EndsAtGearSwitch.

For a multi-segment PreparedPath, replace only candidate.SegmentIndex; copy all other immutable segments. When reconstructing the replaced PreparedDirectionSegment, preserve StartVehicleCurvaturePerMeter exactly.

  • Step 6: Run tests
dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_geometry.ps1

Expected: both pass.

  • Step 7: Commit
git add -- `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateGeometry.cs `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2PathSplicer.cs `
  ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1
git diff --cached --check
git commit -m "feat: build local G2 smoothing candidates"

Task 7: Evaluate candidates against safety and quality gates

Files:

  • Create: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateEvaluator.cs
  • Modify: ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1

Interfaces:

  • Produces:
internal LocalG2CandidateEvaluation Evaluate(
    PreparedPath rawPath,
    PreparedPath currentPath,
    LocalG2SmoothingRegion region,
    LocalG2CandidateGeometry candidate,
    PathSmoothingRequest request,
    LocalG2OptionsSnapshot options,
    CancellationToken cancellationToken);
  • Produces: accepted/rejected status, stable failure reason, safe full path, region metrics, maximum deviation and candidate index.
  • Produces:
internal static LocalG2CandidateEvaluation SelectBest(
    IReadOnlyList<LocalG2CandidateEvaluation> evaluations);
  • Define LocalG2CandidateEvaluation in the evaluator file with these immutable properties:
bool Accepted
PathSmoothingRegionFailureReason FailureReason
int CandidateIndex
PreparedPath SplicedPreparedPath
IReadOnlyList<SmoothedPathPoint> SafePath
IReadOnlyList<SmoothedPathSegment> SafeSegments
double RawPeakCurvatureDerivativePerSquareMeter
double ResultPeakCurvatureDerivativePerSquareMeter
double RawCurvatureVariationCost
double ResultCurvatureVariationCost
double MaximumDeviationMeters
double MinimumBodyClearanceMeters
double MaximumAbsoluteVehicleCurvaturePerMeter
double AbsolutePathLengthChangeMeters
string Reason

Rejected evaluations use SplicedPreparedPath = null and empty immutable safe-path collections.

  • Step 1: Add quality-gate scenarios

Extend the candidate script with deterministic scenarios:

Assert-Equal 'DeviationExceeded' $tooFar.FailureReason `
    'A collision-free candidate more than 0.10 m from the raw window must be rejected.'
Assert-Equal 'CurvatureOvershoot' $overshoot.FailureReason `
    'A candidate outside the raw regional curvature range must be rejected.'
Assert-Equal 'InsufficientImprovement' $noOp.FailureReason `
    'A safe no-op must not be accepted.'
Assert-Equal 'VariationCostRegression' $oscillating.FailureReason `
    'Repeated curvature oscillation must fail the variation-cost gate.'
Assert-Equal 'InsufficientClearance' $lowClearance.FailureReason `
    'A path with less than 0.02 m checked clearance must be rejected.'
Assert-Equal 'Accepted' $improved.Status `
    'A safe candidate with at least 20 percent peak improvement must be accepted.'
Assert-Equal $smallestDeviation.CandidateIndex $best.CandidateIndex `
    'Among sufficient candidates, minimum deviation must win before extra smoothness.'
  • Step 2: Run and verify RED
dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1

Expected: fail on missing evaluation results.

  • Step 3: Implement same-analyzer regional comparison

Extract raw and candidate window point lists with exact interpolated endpoints. Analyze both through PathGeometryAnalyzer using configuration.OutputSpacingMeters.

Accept quality only when:

candidatePeak <= rawPeak * (1d - options.MinimumPeakGradientImprovementRatio)
candidateCost <= rawCost * (1d + options.MaximumVariationCostRegressionRatio)

The first comparison is per region. The complete-path variation cost is checked again in Task 8.

  • Step 4: Implement hard gates

In this order, reject:

  1. non-finite or failed geometry analysis;
  2. curvature beyond vehicle maximum;
  3. curvature outside the raw regional [min, max] plus 1e-6;
  4. maximum candidate-point-to-raw-polyline distance above MaximumDeviationMeters;
  5. complete-path topology/collision validation failure;
  6. checked minimum clearance below MinimumClearanceReserveMeters;
  7. insufficient peak improvement;
  8. variation cost regression.

Compute point-to-segment distance with a clamped projection; do not compare equal normalized indices.

  • Step 5: Implement lexicographic selection

Sort accepted evaluations by:

maximum deviation ascending
peak |dκ/ds| ascending
curvature variation cost ascending
absolute path-length change ascending
candidate index ascending

Do not combine these into an arbitrary weighted score.

  • Step 6: Run focused tests
dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_validation.ps1

Expected: both pass.

  • Step 7: Commit
git add -- `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateEvaluator.cs `
  ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1
git diff --cached --check
git commit -m "feat: validate local G2 candidate quality"

Task 8: Integrate the dedicated pipeline and publish partial results

Files:

  • Create: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2PreSmoothingPipeline.cs
  • Consume unchanged: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2RegionWorkOrder.cs
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingService.cs
  • Create: ClumsyPilot/tests/verify_path_smoothing_local_g2_integration.ps1
  • Modify: ClumsyPilot/tests/verify_path_smoothing_service.ps1

Planned imports:

using System.Collections.ObjectModel;

Interfaces:

  • Consumes: validated request, prepared path and fair raw baseline.
  • Snapshots detector-order regions into an immutable read-only reportOrder before calling LocalG2RegionWorkOrder.TryCreate(...); work-order creation does not mutate this snapshot, and pipeline iteration must not use report-order regions directly.
  • Publishes region reports by indexed iteration over reportOrder in the detector's original ascending order, independently of processing order.
  • Produces:
private readonly LocalG2RegionWorkOrder _workOrder = new LocalG2RegionWorkOrder();

internal PathSmoothingResult Smooth(
    PathSmoothingRequest request,
    PreparedPath preparedPath,
    RawPathBaseline rawBaseline,
    CancellationToken cancellationToken);

  • Guarantees: every valid input publishes one complete safe path; invalid raw input publishes no path.

  • Step 1: Add end-to-end fixture assertions

Use SmoothingScenarioFactory.CreateFixtureRequests(...), rebuild each public PathSmoothingRequest with:

$configuration.Method = [Enum]::Parse($methodType, 'LocalG2Quintic')
$configuration.AllowFallbackToCoarsePath = $true

Assert:

Straight             -> NotNeeded
SingleTurn           -> Complete
LargeHeadingChange   -> Complete
SBend                -> Complete or PartialImprovement
RectangleDetour      -> Complete or PartialImprovement
MultiObstacleDetour  -> Complete or PartialImprovement
ReverseGearSwitch    -> Complete, PartialImprovement, or NotNeeded

For every published path:

Assert-True ($result.Path.Count -gt 0) 'A valid request must publish a complete path.'
Assert-Equal 0 $result.Segments[0].StartIndex 'Segments must start at zero.'
Assert-Equal ($result.Path.Count - 1) $result.Segments[-1].EndIndex 'Segments must cover the path.'
Assert-True $result.Diagnostics.Metrics.IsFeasible 'Published diagnostics must be feasible.'
Assert-True ($result.Diagnostics.Metrics.MinimumBodyClearanceMeters -ge 0.02) `
    'Published paths must retain the configured clearance reserve.'

For Complete and PartialImprovement, require at least one region report with Improved. For PartialImprovement, require at least one RetainedOriginal. Run every fixture twice and compare status, point count, coordinates and region reports.

Create one deterministic same-direction fixture with two disjoint detected regions. Both accepted candidates must change their local replacement length. Assert:

  1. the larger-original-arc region is evaluated first;
  2. the second processed region still matches the intended original front window endpoints;
  3. both reports are Improved;
  4. reports are published in ascending original arc order;
  5. both Local G2 replacements are present in the final full path;
  6. two identical requests produce equal status, candidate indices, report order, point count and point coordinates.

The test must fail if the pipeline replaces workRegions with regions.

Also create two explicit service cases:

# Force every real candidate below the acceptance bar without invalidating the request.
$strict.LocalG2Quintic.MinimumPeakGradientImprovementRatio = 0.99
$unchanged = $service.Smooth($strictRequest, [Threading.CancellationToken]::None)
Assert-Equal 'Unchanged' $unchanged.Status.ToString() `
    'Detected events with no accepted region must publish a verified raw path as Unchanged.'
Assert-True ($unchanged.Path.Count -gt 0) 'Unchanged must retain a complete SQP initial path.'

$cancelSource = [Threading.CancellationTokenSource]::new()
$cancelSource.Cancel()
$cancelled = $service.Smooth($normalRequest, $cancelSource.Token)
Assert-Equal 'Cancelled' $cancelled.Status.ToString() 'Cancellation must propagate through Local G2.'
Assert-Equal 0 $cancelled.Path.Count 'A cancelled run must not publish a partial path.'
  • Step 2: Run and verify RED
dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_local_g2_integration.ps1

Expected: fail because the service cannot resolve LocalG2Quintic.

  • Step 3: Implement sequential regional processing

Pipeline pseudocode must be implemented directly:

IReadOnlyList<LocalG2SmoothingRegion> reportOrder =
    new ReadOnlyCollection<LocalG2SmoothingRegion>(new List<LocalG2SmoothingRegion>(regions));

if (!_workOrder.TryCreate(
        reportOrder,
        out IReadOnlyList<LocalG2SmoothingRegion> workRegions,
        out string orderReason))
{
    return PathSmoothingResult.Failure(
        PathSmoothingStatus.Failed,
        new PathSmoothingDiagnostics(
            new PathQualityMetrics(),
            TimeSpan.Zero,
            0,
            0d,
            orderReason));
}

PreparedPath current = preparedPath;
var reportsByRegion = new Dictionary<LocalG2SmoothingRegion, PathSmoothingRegionReport>();
int improvedCount = 0;

foreach (LocalG2SmoothingRegion region in workRegions)
{
    cancellationToken.ThrowIfCancellationRequested();
    IReadOnlyList<LocalG2CandidateGeometry> candidates =
        _builder.Build(
            preparedPath.Segments[region.SegmentIndex],
            region,
            outputSpacing,
            options,
            cancellationToken);
    var evaluations = new List<LocalG2CandidateEvaluation>();
    foreach (LocalG2CandidateGeometry candidate in candidates)
    {
        evaluations.Add(
            _evaluator.Evaluate(
                preparedPath,
                current,
                region,
                candidate,
                request,
                options,
                cancellationToken));
    }

    LocalG2CandidateEvaluation best =
        LocalG2CandidateEvaluator.SelectBest(evaluations);
    if (best != null && best.Accepted)
    {
        current = best.SplicedPreparedPath;
        improvedCount++;
        reportsByRegion.Add(region, CreateImprovedReport(region, best));
    }
    else
    {
        reportsByRegion.Add(
            region,
            CreateRetainedReport(region, evaluations));
    }
}

var reports = new List<PathSmoothingRegionReport>(reportOrder.Count);
for (int reportIndex = 0; reportIndex < reportOrder.Count; reportIndex++)
    reports.Add(reportsByRegion[reportOrder[reportIndex]]);

Regions are disjoint after merging, so successful earlier replacements remain when a later region fails.

  • Step 4: Perform final full-path analysis and rollback

Analyze and validate current. Require:

finalVariationCost <= rawVariationCost * (1d + options.MaximumVariationCostRegressionRatio)

If final validation fails, roll back improved regions in reverse acceptance order, marking GlobalValidationRollback, until validation passes. If all are rolled back, publish the verified raw baseline as Unchanged. If raw baseline was not valid, publish Failed with an empty path.

  • Step 5: Derive final status exactly
if (transitions.Count == 0) status = PathSmoothingStatus.NotNeeded;
else if (improvedCount == regions.Count) status = PathSmoothingStatus.Complete;
else if (improvedCount > 0) status = PathSmoothingStatus.PartialImprovement;
else status = PathSmoothingStatus.Unchanged;

Complete and PartialImprovement cannot be emitted with zero improved reports.

  • Step 6: Dispatch without disturbing legacy methods

In PathSmoothingService:

  1. validate common inputs;
  2. validate only LocalG2QuinticOptions when method is LocalG2Quintic;
  3. prepare and build the fair raw baseline once;
  4. branch to _localG2Pipeline.Smooth(...);
  5. otherwise continue through the existing IPathSmoother runner.

Do not add Local G2 to PathSmoothingComparisonRequest.DefaultMethods.

  • Step 7: Run focused and legacy service tests
dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_local_g2_integration.ps1
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_service.ps1
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_runner.ps1
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_integration.ps1

Expected: all scripts pass. The old integration script may continue reporting zero successful legacy methods on difficult fixtures; that is outside this task.

  • Step 8: Commit
git add -- `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2PreSmoothingPipeline.cs `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Facade/PathSmoothingService.cs `
  ClumsyPilot/tests/verify_path_smoothing_local_g2_integration.ps1 `
  ClumsyPilot/tests/verify_path_smoothing_service.ps1
git diff --cached --check
git commit -m "feat: publish local G2 presmoothing results"

Task 9: Document the main route and run the complete regression suite

Files:

  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/README.md
  • Modify: ClumsyPilot/tests/verify_path_smoothing_documentation.ps1

Interfaces:

  • Documents: module role as SQP warm-start generator.

  • Documents: statuses, defaults, output fields and non-goals.

  • Does not claim old comparison algorithms are production choices.

  • Step 1: Add failing documentation assertions

Require README sections or exact terms:

$required = @(
    '局部 G2 预平滑',
    'SQP 初始路径',
    'Complete',
    'PartialImprovement',
    'NotNeeded',
    'Unchanged',
    '0.20 m',
    '0.50 m',
    '0.80 m',
    '0.10 m',
    'dκ/ds',
    '不跨换向点',
    '不输出速度'
)
foreach ($text in $required) {
    if (-not $readme.Contains($text)) { throw "PathSmoothing README must contain: $text" }
}
  • Step 2: Run and verify RED
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_documentation.ps1

Expected: fail on the first missing Local G2 term.

  • Step 3: Update README

Document:

  • Hybrid A* is G1 but can have primitive-boundary curvature jumps.

  • Local G2 is the main pre-SQP route.

  • hard direction segmentation and stopped gear-switch semantics;

  • window defaults and maximum deviation;

  • fair same-analyzer metrics;

  • complete/partial/not-needed/unchanged/failed meanings;

  • spatial output fields including κ and dκ/ds;

  • speed planning owns 1 m/s, 60°/s and future speed configurations;

  • old algorithms remain offline comparison only.

  • Step 4: Run all PathSmoothing checks

Run:

dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore
$scripts = @(
  'verify_path_smoothing_contracts.ps1',
  'verify_path_smoothing_algorithm_input.ps1',
  'verify_path_smoothing_geometry.ps1',
  'verify_path_smoothing_validation.ps1',
  'verify_path_smoothing_runner.ps1',
  'verify_path_smoothing_service.ps1',
  'verify_path_smoothing_bspline.ps1',
  'verify_path_smoothing_bezier.ps1',
  'verify_path_smoothing_quintic.ps1',
  'verify_path_smoothing_comparison.ps1',
  'verify_path_smoothing_integration.ps1',
  'verify_path_smoothing_fixtures.ps1',
  'verify_path_smoothing_svg_csv.ps1',
  'verify_path_smoothing_png.ps1',
  'verify_path_smoothing_documentation.ps1',
  'verify_path_smoothing_local_g2_detection.ps1',
  'verify_path_smoothing_local_g2_curve.ps1',
  'verify_path_smoothing_local_g2_candidates.ps1',
  'verify_path_smoothing_local_g2_integration.ps1'
)
foreach ($script in $scripts) {
  powershell -ExecutionPolicy Bypass -File (Join-Path 'ClumsyPilot/tests' $script)
  if ($LASTEXITCODE -ne 0) { exit $LASTEXITCODE }
}

Expected: build exits 0; every script prints its success message and exits 0.

  • Step 5: Run CoarsePath regression checks
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_coarse_path_integration.ps1
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_coarse_path_collision.ps1
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_coarse_path_search.ps1

Expected: all pass.

  • Step 6: Inspect fixture outcomes

Run:

powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_local_g2_integration.ps1

Record the status, improved-region count, retained-region count, peak |dκ/ds|, variation cost, maximum deviation and minimum clearance for every fixture. Do not weaken assertions merely to make a fixture green; if SingleTurn or LargeHeadingChange is not Complete, return to Tasks 4–7 and correct the algorithm.

  • Step 7: Commit documentation and final verification changes
git add -- `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/README.md `
  ClumsyPilot/tests/verify_path_smoothing_documentation.ps1
git diff --cached --check
git commit -m "docs: describe local G2 presmoothing workflow"
  • Step 8: Final repository checks
git diff --check
git status --short
git log --oneline -10

Expected:

  • no whitespace errors;
  • no staged files;
  • only pre-existing unrelated working-tree changes remain;
  • the task commits appear in order.

Do not report implementation complete unless the full build and every listed verification script have fresh exit code 0.

Reference in New Issue View Git Blame Copy Permalink