First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Files
32083db4d09447745f7a58f0086f006326ace5e9
ParkingRobot/docs/superpowers/plans/2026-07-31-local-g2-pre-task8-stabilization.md
T

59 KiB
Raw Blame History

Local G2 Pre-Task-8 Stabilization 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: 修复进入任务 8 前的曲率基线误判、局部窗口总长度错误和多区域弧长错位风险，并用失败先行回归证明安全边界没有被削弱。

Architecture: PathGeometryAnalyzer 首先通过受认证的圆弧—弦长估计器消除极限圆弧的离散高估；若认证失败，或原始基线的共享重建因非有限/超限曲率被未改动验证器拒绝而可信粗路径曲率仍有限且在车辆上限内，只让 RawPathBaselineBuilder 使用粗路径自带曲率和弧长，并重新通过完整验证。LocalG2WindowPlanner 以“是否能生成覆盖全部事件的合法候选”决定分组，新的 LocalG2RegionWorkOrder 只负责把报告顺序转换为同方向段从后向前的工作顺序。

Tech Stack: C# 10、.NET Standard 2.0、PowerShell 反射测试、现有 dotnet build 与 verify_path_smoothing_*.ps1 回归脚本。

Global Constraints

  • MinimumWindowLengthMeters = 0.20、PreferredWindowLengthMeters = 0.50、MaximumWindowLengthMeters = 0.80 都表示左窗口加右窗口的总长度。
  • 不修改车辆最大曲率、SmoothedPathValidator 的 1e-6 容差、碰撞门限、净空门限或候选质量门。
  • 不新增 RectangleDetour 碰撞位置、最小净空或可视化诊断作为本阶段验收内容。
  • 不创建 LocalG2PreSmoothingPipeline.cs，不修改 PathSmoothingService 分派，不把 LocalG2Quintic 接入正式服务。
  • 工作顺序为方向段编号升序、同一方向段原始局部弧长降序；报告顺序仍为检测器的原始升序。
  • 候选从不可变原始路径构造；任务 8 才能把评价和拼接接到当前路径并完成端到端证明。
  • 采用 TDD：每项行为先看到预期失败，再写最小实现；每个任务只暂存其列出的文件，保留工作区其他修改。

File Structure

  • ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalyzer.cs
    • 共享几何重采样、航向展开、圆弧—弦长离散曲率和曲率导数统计。
  • ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/RawPathBaselineBuilder.cs
    • 继续使用共享分析器；仅在认证失败，或原始基线重建因非有限/超限曲率被验证器拒绝而可信粗路径曲率仍在车辆上限内时，承载粗路径专用限定回退。
  • ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2WindowPlanner.cs
    • 生成总长度受限的窗口变体，并以变体可生成性决定事件分组和区域包络。
  • ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2SmoothingRegion.cs
    • 保留区域与窗口不可变数据合同，不新增第二套长度定义。
  • ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2RegionWorkOrder.cs
    • 新建；验证区域并生成确定性工作顺序，不改变输入报告顺序。
  • ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs
    • 只扩展现有窄范围 TestHooks，承载双区域拼接证明，不改变候选构造主逻辑。
  • ClumsyPilot/tests/verify_path_smoothing_geometry.ps1
    • 极限/超限圆弧、端点、倒车、分支切换和解析五次曲线认证。
  • ClumsyPilot/tests/verify_path_smoothing_integration.ps1
    • 保持 RectangleDetour 原始基线和全场景集成验收。
  • ClumsyPilot/tests/verify_path_smoothing_local_g2_detection.ps1
    • 0.80 m 总窗口、分组、边界偏置、包络和确定性回归。
  • ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1
    • 区域工作顺序和双窗口拼接回归。
  • docs/superpowers/plans/2026-07-30-local-g2-path-presmoothing.md
    • 更新任务 8，使其显式消费工作顺序组件并包含真实双区域硬门。

Task 1: Certify and Fix the Shared Curvature Estimator

Files:

  • Modify: ClumsyPilot/tests/verify_path_smoothing_geometry.ps1
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalyzer.cs
  • Verify unchanged: ClumsyPilot/tests/verify_path_smoothing_validation.ps1
  • Verify unchanged: ClumsyPilot/tests/verify_path_smoothing_integration.ps1
  • Conditional modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/RawPathBaselineBuilder.cs

Interfaces:

  • Consumes: PreparedDirectionSegment.Points,连续展开后的车辆航向、重采样点位置和 TravelDirection。
  • Produces: 保持现有公开签名不变：
public bool TryAnalyze(
    IReadOnlyList<PreparedDirectionSegment> candidateSegments,
    double spacingMeters,
    out PathGeometryAnalysis analysis,
    out string reason);
  • Internal rule: for sample pair (left, right),
deltaHeading = unwrappedHeading[right] - unwrappedHeading[left]
chordLength = distance(position[left], position[right])
kappa = 2 * sin(deltaHeading / 2) / chordLength

  • Safety gate: abs(deltaHeading) >= π、非有限弦长或退化弦长必须稳定失败。

  • Step 1: Add exact-circle and analytic-reference helpers to the geometry script

Insert these complete helpers after Invoke-Analysis in ClumsyPilot/tests/verify_path_smoothing_geometry.ps1:

function Normalize-Angle([double]$Angle) {
    return [Math]::Atan2([Math]::Sin($Angle), [Math]::Cos($Angle))
}

function New-ConstantCurvatureSegment(
    [double]$VehicleCurvature,
    $Direction,
    [int]$IntervalCount = 20,
    [double]$ChordLength = 0.05) {
    $radius = 1.0 / [Math]::Abs($VehicleCurvature)
    $geometricSign = if ($Direction.ToString() -eq 'Forward') {
        [Math]::Sign($VehicleCurvature)
    } else {
        -[Math]::Sign($VehicleCurvature)
    }
    $deltaTheta = $geometricSign * 2.0 * [Math]::Asin($ChordLength / (2.0 * $radius))
    $points = New-Object System.Collections.Generic.List[object]
    for ($index = 0; $index -le $IntervalCount; $index++) {
        $theta = $index * $deltaTheta
        $x = 2.0 + $geometricSign * $radius * [Math]::Sin($theta)
        $y = 2.0 + $geometricSign * $radius * (1.0 - [Math]::Cos($theta))
        $travelHeading = $theta
        $vehicleHeading = if ($Direction.ToString() -eq 'Forward') {
            $travelHeading
        } else {
            $travelHeading + [Math]::PI
        }
        [void]$points.Add((New-GeometryPoint `
            $x $y ($index * $ChordLength) (Normalize-Angle $vehicleHeading) $vehicleHeading))
    }
    return New-DirectionSegment 0 $Direction $points.ToArray()
}

function Get-LegacyMaximumCurvature($Analysis) {
    $maximum = 0.0
    foreach ($segment in $Analysis.Segments) {
        for ($index = $segment.StartIndex; $index -le $segment.EndIndex; $index++) {
            $leftIndex = if ($index -eq $segment.StartIndex) { $index } else { $index - 1 }
            $rightIndex = if ($index -eq $segment.EndIndex) { $index } else { $index + 1 }
            if ($leftIndex -eq $rightIndex) { continue }
            $arc = 0.0
            for ($pointIndex = $leftIndex + 1; $pointIndex -le $rightIndex; $pointIndex++) {
                $left = $Analysis.Path[$pointIndex - 1]
                $right = $Analysis.Path[$pointIndex]
                $dx = $right.X - $left.X
                $dy = $right.Y - $left.Y
                $arc += [Math]::Sqrt($dx * $dx + $dy * $dy)
            }
            $delta = $Analysis.Path[$rightIndex].UnwrappedHeading -
                $Analysis.Path[$leftIndex].UnwrappedHeading
            $maximum = [Math]::Max($maximum, [Math]::Abs($delta / $arc))
        }
    }
    return $maximum
}

function New-AnalyticQuinticSegment(
    [double[]]$Coefficients,
    [double]$DistributionPower) {
    $points = New-Object System.Collections.Generic.List[object]
    for ($index = 0; $index -le 400; $index++) {
        $u = $index / 400.0
        $t = [Math]::Pow($u, $DistributionPower)
        $c2 = $Coefficients[0]
        $c3 = $Coefficients[1]
        $c4 = $Coefficients[2]
        $c5 = $Coefficients[3]
        $y = $c2 * $t * $t + $c3 * [Math]::Pow($t, 3) +
            $c4 * [Math]::Pow($t, 4) + $c5 * [Math]::Pow($t, 5)
        $dy = 2.0 * $c2 * $t + 3.0 * $c3 * $t * $t +
            4.0 * $c4 * [Math]::Pow($t, 3) + 5.0 * $c5 * [Math]::Pow($t, 4)
        $heading = [Math]::Atan2($dy, 1.0)
        [void]$points.Add((New-GeometryPoint (1.0 + $t) (1.0 + $y) $t $heading $heading))
    }
    return New-DirectionSegment 0 $forward $points.ToArray()
}

function Get-AnalyticQuinticMaximum(
    [double[]]$Coefficients) {
    $maximum = 0.0
    for ($index = 0; $index -le 20000; $index++) {
        $t = $index / 20000.0
        $c2 = $Coefficients[0]
        $c3 = $Coefficients[1]
        $c4 = $Coefficients[2]
        $c5 = $Coefficients[3]
        $dy = 2.0 * $c2 * $t + 3.0 * $c3 * $t * $t +
            4.0 * $c4 * [Math]::Pow($t, 3) + 5.0 * $c5 * [Math]::Pow($t, 4)
        $ddy = 2.0 * $c2 + 6.0 * $c3 * $t +
            12.0 * $c4 * $t * $t + 20.0 * $c5 * [Math]::Pow($t, 3)
        $curvature = [Math]::Abs($ddy / [Math]::Pow(1.0 + $dy * $dy, 1.5))
        $maximum = [Math]::Max($maximum, $curvature)
    }
    return $maximum
}

function New-PreparedPathFromSegment($Segment) {
    $segments = [Array]::CreateInstance($segmentType, 1)
    $segments.SetValue($Segment, 0)
    return [Activator]::CreateInstance(
        $preparedPathType,
        [object[]]@(,$segments))
}

function Invoke-AnalyzedValidation($Analysis, $Original, $Vehicle) {
    $arguments = [object[]]@(
        $Analysis.Path,
        $Analysis.Segments,
        $Original,
        (New-EmptyGeometryMap),
        $Vehicle,
        [double]0.05,
        $null,
        [double]0.0,
        $null)
    $accepted = $validateMethod.Invoke($validator, $arguments)
    return [PSCustomObject]@{
        Accepted = $accepted
        Reason = $arguments[8]
    }
}
  • Step 2: Add the failing certification assertions

Add these declarations beside the existing reflected geometry types and analyzer setup:

$validatorType = Get-RequiredType (
    $root + 'Validation.SmoothedPathValidator')
$validator = [Activator]::CreateInstance($validatorType)
$validateMethod = $validatorType.GetMethod('TryValidate')
Assert-True ($null -ne $validateMethod) `
    'The curvature certification must use the existing full path validator.'

Insert this block after the existing forward/reverse circle assertions:

$maximumAllowedCurvature = 5.0 / 6.0
$certificationVehicle = [Activator]::CreateInstance($vehicleType)
$certificationVehicle.LengthMeters = 0.20
$certificationVehicle.WidthMeters = 0.20
$certificationVehicle.SafetyMarginMeters = 0.0
$certificationVehicle.MaximumCurvaturePerMeter = $maximumAllowedCurvature
foreach ($direction in @($forward, $reverse)) {
    $signedCurvature = if ($direction.ToString() -eq 'Forward') {
        $maximumAllowedCurvature
    } else {
        -$maximumAllowedCurvature
    }
    $limitSegment = New-ConstantCurvatureSegment $signedCurvature $direction
    $limitAnalysis = Invoke-Analysis @($limitSegment) 0.05
    foreach ($point in $limitAnalysis.Path) {
        Assert-True ([Math]::Abs($point.VehicleCurvature) -le $maximumAllowedCurvature + 1e-9) `
            'Every endpoint and interior sample on an exact-limit circle must remain within the limit.'
    }
    $limitValidation = Invoke-AnalyzedValidation `
        $limitAnalysis `
        (New-PreparedPathFromSegment $limitSegment) `
        $certificationVehicle
    Assert-True $limitValidation.Accepted `
        ('The existing validator must accept an exact-limit circle. Reason=' + $limitValidation.Reason)
}

$quinticCases = @(
    [PSCustomObject]@{ Name = 'SBend'; Coefficients = [double[]]@(0.0, 0.30, -0.45, 0.18); Power = 1.0 },
    [PSCustomObject]@{ Name = 'EndpointPeak'; Coefficients = [double[]]@(0.18, -0.12, 0.0, 0.0); Power = 1.0 },
    [PSCustomObject]@{ Name = 'NonUniformFinalInterval'; Coefficients = [double[]]@(-0.12, 0.36, -0.30, 0.08); Power = 1.7 }
)
foreach ($case in $quinticCases) {
    $analysis = Invoke-Analysis @(
        (New-AnalyticQuinticSegment $case.Coefficients $case.Power)
    ) 0.013
    $analyticMaximum = Get-AnalyticQuinticMaximum $case.Coefficients
    $legacyMaximum = Get-LegacyMaximumCurvature $analysis
    $newUnderestimate = [Math]::Max(
        0.0, $analyticMaximum - $analysis.MaximumAbsoluteVehicleCurvaturePerMeter)
    $legacyUnderestimate = [Math]::Max(0.0, $analyticMaximum - $legacyMaximum)
    Assert-True ($newUnderestimate -le $legacyUnderestimate + 1e-6) `
        "$($case.Name) must not increase one-sided maximum-curvature underestimation."
}

$ambiguousTurn = New-DirectionSegment 0 $forward @(
    (New-GeometryPoint 0.0 0.0 0.0 0.0 0.0),
    (New-GeometryPoint 1.0 0.0 1.0 ([Math]::PI) ([Math]::PI)))
Invoke-RejectedAnalysis @($ambiguousTurn) `
    'A curvature sample spanning an ambiguous pi-radian heading change must be rejected.'

The existing verify_path_smoothing_validation.ps1 assertion A smoothing candidate above vehicle maximum curvature must be rejected remains the independent validator gate. After the estimator implementation, also add an analyzed over-limit circle check:

$overLimitSegment = New-ConstantCurvatureSegment `
    ($maximumAllowedCurvature + 0.01) `
    $forward
$overLimitAnalysis = Invoke-Analysis @($overLimitSegment) 0.05
Assert-True (
    $overLimitAnalysis.MaximumAbsoluteVehicleCurvaturePerMeter -gt
        $maximumAllowedCurvature + 1e-6
) 'An actually over-limit circle must remain measurably above the validator tolerance.'
$overLimitValidation = Invoke-AnalyzedValidation `
    $overLimitAnalysis `
    (New-PreparedPathFromSegment $overLimitSegment) `
    $certificationVehicle
Assert-False $overLimitValidation.Accepted `
    'The existing validator must reject an actually over-limit analyzed circle.'
  • Step 3: Run the tests to verify RED

Run:

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_integration.ps1

Expected:

  • build succeeds;

  • geometry fails because the current deltaHeading / polylineArcLength estimate exceeds the exact circle by more than 1e-9;

  • integration fails at RectangleDetour with Expected=Success Actual=InvalidInput.

  • Step 4: Implement the shared circle-chord estimator

Replace the curvature-assignment loop in TryAnalyzeSegment with:

for (int index = 0; index < count; index++)
{
    if (count == 1)
    {
        geometricCurvatures[index] = 0d;
        continue;
    }

    int leftIndex = index == 0 ? 0 : index - 1;
    int rightIndex = index == count - 1 ? count - 1 : index + 1;
    if (!TryEstimateGeometricCurvature(
            samples,
            unwrappedHeadings,
            leftIndex,
            rightIndex,
            out geometricCurvatures[index],
            out reason))
    {
        return false;
    }
}

Add this complete private method next to Distance:

private static bool TryEstimateGeometricCurvature(
    IReadOnlyList<SmoothingPoint2D> samples,
    IReadOnlyList<double> unwrappedHeadings,
    int leftIndex,
    int rightIndex,
    out double curvature,
    out string reason)
{
    curvature = 0d;
    reason = string.Empty;
    if (samples == null || unwrappedHeadings == null ||
        leftIndex < 0 || rightIndex <= leftIndex ||
        rightIndex >= samples.Count || rightIndex >= unwrappedHeadings.Count)
    {
        reason = "候选路径曲率采样索引无效。";
        return false;
    }

    double chordLength = Distance(samples[leftIndex], samples[rightIndex]);
    double deltaHeading = unwrappedHeadings[rightIndex] - unwrappedHeadings[leftIndex];
    if (!NumericGuard.IsPositiveFinite(chordLength) ||
        !NumericGuard.IsFinite(deltaHeading) ||
        Math.Abs(deltaHeading) >= Math.PI)
    {
        reason = "候选路径曲率采样包含退化弦或无法唯一展开的转角。";
        return false;
    }

    curvature = 2d * Math.Sin(0.5d * deltaHeading) / chordLength;
    if (!NumericGuard.IsFinite(curvature))
    {
        reason = "候选路径曲率计算产生了非法数值。";
        return false;
    }
    return true;
}

Do not change the direction-sign conversion, start steering-curvature override, curvature-derivative denominator, validator tolerance, or vehicle limits.

  • Step 5: Run the certification and P0 integration gate

Run:

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_bezier.ps1
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_bspline.ps1
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_quintic.ps1
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_integration.ps1

Expected: all commands pass; RectangleDetour raw baseline reports Success.

If the analytic one-sided-underestimation assertion fails, do not change its 1e-6 allowance or any safety threshold. Also execute the conditional fallback in Step 6 when the shared-analyzer reconstruction of the raw baseline is rejected by the unchanged validator specifically for nonfinite or over-limit reconstructed curvature, while the corresponding trusted coarse VehicleCurvature is finite and within the vehicle limit.

  • Step 6: Conditional raw-baseline fallback gate

This step is skipped unless either gate above is met. It applies only to the raw baseline: candidate paths keep the shared analyzer and every safety threshold, including the 1e-6 validator tolerance, remains unchanged.

  1. retain the shared chord-based estimator even when the raw-baseline reconstructed curvature invokes this fallback;
  2. keep the exact-limit and over-limit analyzer/validator tests;
  3. in RawPathBaselineBuilder.TryCreate, attempt the shared analysis and existing full validator first; invoke the trusted raw analysis only when that validator rejects specifically for nonfinite or over-limit reconstructed curvature and the trusted coarse curvatures are finite and within the vehicle limit;
  4. implement the trusted raw analysis with:
private static bool TryCreateCoarseAnalysis(
    PathSmoothingRequest request,
    PreparedPath preparedPath,
    out PathGeometryAnalysis analysis,
    out string reason)
{
    analysis = null;
    reason = string.Empty;
    if (request.CoarsePath == null || request.Segments == null ||
        request.CoarsePath.Count == 0 ||
        request.Segments.Count != preparedPath.Segments.Count)
    {
        reason = "原始粗路径基线的点或方向段无效。";
        return false;
    }

    var path = new List<SmoothedPathPoint>(request.CoarsePath.Count);
    var segments = new List<SmoothedPathSegment>(request.Segments.Count);
    double maximumCurvature = 0d;
    double maximumDerivative = 0d;
    double curvatureSquareSum = 0d;
    double totalVariation = 0d;
    double variationEnergy = 0d;
    double minimumClearance = double.PositiveInfinity;

    for (int segmentIndex = 0; segmentIndex < request.Segments.Count; segmentIndex++)
    {
        CoarsePath.PathSegment segment = request.Segments[segmentIndex];
        if (segment == null || segment.SegmentIndex != segmentIndex ||
            segment.StartIndex < 0 || segment.EndIndex < segment.StartIndex ||
            segment.EndIndex >= request.CoarsePath.Count)
        {
            reason = "原始粗路径基线的方向段索引无效。";
            return false;
        }

        for (int pointIndex = segment.StartIndex; pointIndex <= segment.EndIndex; pointIndex++)
        {
            CoarsePath.CoarsePathPoint point = request.CoarsePath[pointIndex];
            int leftIndex = pointIndex == segment.StartIndex ? pointIndex : pointIndex - 1;
            int rightIndex = pointIndex == segment.EndIndex ? pointIndex : pointIndex + 1;
            double derivative = 0d;
            if (leftIndex != rightIndex)
            {
                double deltaArc = request.CoarsePath[rightIndex].ArcLength -
                    request.CoarsePath[leftIndex].ArcLength;
                double deltaCurvature = request.CoarsePath[rightIndex].VehicleCurvature -
                    request.CoarsePath[leftIndex].VehicleCurvature;
                if (!NumericGuard.IsPositiveFinite(deltaArc) ||
                    !NumericGuard.IsFinite(deltaCurvature))
                {
                    reason = "原始粗路径基线无法计算有限的曲率导数。";
                    return false;
                }
                derivative = deltaCurvature / deltaArc;
            }

            double directionSign = point.Direction == CoarsePath.TravelDirection.Forward ? 1d : -1d;
            double geometricCurvature = directionSign * point.VehicleCurvature;
            if (!NumericGuard.IsFinite(geometricCurvature) ||
                !NumericGuard.IsFinite(derivative))
            {
                reason = "原始粗路径基线包含非法曲率数值。";
                return false;
            }

            path.Add(new SmoothedPathPoint(
                point.X,
                point.Y,
                point.Heading,
                point.UnwrappedHeading,
                point.ArcLength,
                point.Direction,
                geometricCurvature,
                point.VehicleCurvature,
                derivative,
                point.BodyClearance,
                point.IsGearSwitchPoint,
                point.IsGearSwitchPoint
                    ? SmoothedPathPointSource.GearSwitch
                    : SmoothedPathPointSource.CoarsePathFallback));

            maximumCurvature = Math.Max(maximumCurvature, Math.Abs(point.VehicleCurvature));
            maximumDerivative = Math.Max(maximumDerivative, Math.Abs(derivative));
            curvatureSquareSum += point.VehicleCurvature * point.VehicleCurvature;
            minimumClearance = Math.Min(minimumClearance, point.BodyClearance);
            if (pointIndex > segment.StartIndex)
            {
                CoarsePath.CoarsePathPoint previous = request.CoarsePath[pointIndex - 1];
                double deltaArc = point.ArcLength - previous.ArcLength;
                double deltaCurvature = geometricCurvature -
                    (previous.Direction == CoarsePath.TravelDirection.Forward ? 1d : -1d) *
                    previous.VehicleCurvature;
                if (!NumericGuard.IsPositiveFinite(deltaArc) ||
                    !NumericGuard.IsFinite(deltaCurvature))
                {
                    reason = "原始粗路径基线的曲率变化统计无效。";
                    return false;
                }
                totalVariation += Math.Abs(deltaCurvature);
                variationEnergy += deltaCurvature * deltaCurvature / deltaArc;
            }
        }

        segments.Add(new SmoothedPathSegment(
            segment.SegmentIndex,
            segment.Direction,
            segment.StartIndex,
            segment.EndIndex,
            segment.StartsAtGearSwitch,
            segment.EndsAtGearSwitch));
    }

    double pathLength = request.CoarsePath[request.CoarsePath.Count - 1].ArcLength;
    double rmsCurvature = Math.Sqrt(curvatureSquareSum / request.CoarsePath.Count);
    analysis = new PathGeometryAnalysis(
        path,
        segments,
        pathLength,
        maximumCurvature,
        maximumDerivative,
        rmsCurvature,
        totalVariation,
        variationEnergy,
        minimumClearance);
    return true;
}

Add these using directives if the fallback is used:

using System;
using MultiWheelC.TrajectoryPlanning.Utils;
using CoarsePath = MultiWheelC.TrajectoryPlanning.CoarsePath;

Run the same commands from Step 5. Expected: RectangleDetour and the raw exact-limit baseline pass; the unchanged candidate analyzer and validator still reject over-limit candidates.

  • Step 7: Commit the successful P0 branch

For the shared estimator branch:

git add -- `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/Processing/PathGeometryAnalyzer.cs `
  ClumsyPilot/tests/verify_path_smoothing_geometry.ps1
git diff --cached --check
git commit -m "fix: certify path curvature estimation"

For the conditional fallback branch, include RawPathBaselineBuilder.cs in the same git add command and use:

git commit -m "fix: preserve trusted raw path curvature"

Task 2: Enforce Total Window Length and Feasible Event Grouping

Files:

  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2WindowPlanner.cs
  • Modify: ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2SmoothingRegion.cs
  • Modify: ClumsyPilot/tests/verify_path_smoothing_local_g2_detection.ps1

Interfaces:

  • Consumes: sorted CurvatureTransition values, per-segment local length, and LocalG2OptionsSnapshot.
  • Produces: unchanged method contract:
internal bool TryPlan(
    PreparedPath originalPath,
    IReadOnlyList<CurvatureTransition> transitions,
    LocalG2OptionsSnapshot options,
    out IReadOnlyList<LocalG2SmoothingRegion> regions,
    out string reason);

  • Guarantee: every emitted region has at least one LocalG2WindowVariant; every variant satisfies EndArcLengthMeters - StartArcLengthMeters <= MaximumWindowLengthMeters + 1e-9.

  • Step 1: Add window-semantics scenarios to the planner test hook

Add a nested public TestHooks class to LocalG2WindowPlanner with this stable entry point:

public static class TestHooks
{
    public static WindowPlanningTestSnapshot Execute(string scenario)
    {
        if (string.IsNullOrWhiteSpace(scenario))
            throw new ArgumentException("A scenario is required.", nameof(scenario));

        IReadOnlyList<CurvatureTransition> transitions;
        double segmentLength;
        switch (scenario)
        {
            case "SeparatedByOneMeter":
                transitions = new[]
                {
                    Transition(0.2d, 0),
                    Transition(1.2d, 1),
                };
                segmentLength = 1.4d;
                break;
            case "Mergeable":
                transitions = new[]
                {
                    Transition(0.4d, 0),
                    Transition(0.7d, 1),
                };
                segmentLength = 1.4d;
                break;
            case "ThreeEventPartition":
                transitions = new[]
                {
                    Transition(0.2d, 0),
                    Transition(0.6d, 1),
                    Transition(1.2d, 2),
                };
                segmentLength = 1.4d;
                break;
            case "NearBoundary":
                transitions = new[] { Transition(0.1d, 0) };
                segmentLength = 1d;
                break;
            default:
                throw new ArgumentOutOfRangeException(nameof(scenario));
        }

        var planner = new LocalG2WindowPlanner();
        if (!planner.TryPlan(
                CreatePreparedPath(segmentLength),
                transitions,
                new LocalG2OptionsSnapshot(new PathSmoothingConfiguration()),
                out IReadOnlyList<LocalG2SmoothingRegion> regions,
                out string reason))
        {
            throw new InvalidOperationException(reason);
        }

        double maximumLength = 0d;
        bool exactEnvelope = true;
        var counts = new List<string>(regions.Count);
        var signature = new List<string>();
        for (int regionIndex = 0; regionIndex < regions.Count; regionIndex++)
        {
            LocalG2SmoothingRegion region = regions[regionIndex];
            counts.Add(region.Transitions.Count.ToString());
            double minimumStart = double.PositiveInfinity;
            double maximumEnd = double.NegativeInfinity;
            for (int variantIndex = 0; variantIndex < region.WindowVariants.Count; variantIndex++)
            {
                LocalG2WindowVariant variant = region.WindowVariants[variantIndex];
                maximumLength = Math.Max(
                    maximumLength,
                    variant.EndArcLengthMeters - variant.StartArcLengthMeters);
                minimumStart = Math.Min(minimumStart, variant.StartArcLengthMeters);
                maximumEnd = Math.Max(maximumEnd, variant.EndArcLengthMeters);
                signature.Add(
                    region.SegmentIndex + ":" +
                    variant.CandidateIndex + ":" +
                    variant.StartArcLengthMeters.ToString("R") + ":" +
                    variant.EndArcLengthMeters.ToString("R"));
            }
            exactEnvelope &= Math.Abs(region.MaximumStartArcLengthMeters - minimumStart) <= 1e-9d;
            exactEnvelope &= Math.Abs(region.MaximumEndArcLengthMeters - maximumEnd) <= 1e-9d;
        }

        LocalG2WindowVariant first = regions[0].WindowVariants[0];
        return new WindowPlanningTestSnapshot(
            regions.Count,
            string.Join(",", counts),
            maximumLength,
            exactEnvelope,
            first.LeftWindowLengthMeters,
            first.RightWindowLengthMeters,
            string.Join("|", signature));
    }

    public sealed class WindowPlanningTestSnapshot
    {
        internal WindowPlanningTestSnapshot(
            int regionCount,
            string transitionCounts,
            double maximumWindowLength,
            bool exactEnvelope,
            double firstLeftLength,
            double firstRightLength,
            string signature)
        {
            RegionCount = regionCount;
            TransitionCounts = transitionCounts;
            MaximumWindowLength = maximumWindowLength;
            ExactEnvelope = exactEnvelope;
            FirstLeftLength = firstLeftLength;
            FirstRightLength = firstRightLength;
            Signature = signature;
        }

        public int RegionCount { get; }
        public string TransitionCounts { get; }
        public double MaximumWindowLength { get; }
        public bool ExactEnvelope { get; }
        public double FirstLeftLength { get; }
        public double FirstRightLength { get; }
        public string Signature { get; }
    }

    private static CurvatureTransition Transition(double arcLength, int index)
    {
        return new CurvatureTransition(
            0,
            index,
            index + 1,
            arcLength,
            arcLength,
            0d,
            0d,
            index % 2 == 0 ? 0d : 0.5d,
            index % 2 == 0 ? 0.5d : 0d);
    }

    private static PreparedPath CreatePreparedPath(double length)
    {
        var points = new[]
        {
            new SmoothingPoint2D(
                0d, 0d, 0d, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
            new SmoothingPoint2D(
                length, 0d, length, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
        };
        return new PreparedPath(new[]
        {
            new PreparedDirectionSegment(
                0, TravelDirection.Forward, points, false, false),
        });
    }
}

The file already imports System, collections, processing and utility namespaces; add MultiWheelC.TrajectoryPlanning.CoarsePath because the test hook names TravelDirection.

  • Step 2: Add failing reflection assertions

Append to verify_path_smoothing_local_g2_detection.ps1:

$plannerType = Get-RequiredType 'MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2.LocalG2WindowPlanner'
$plannerHooksType = $plannerType.GetNestedType('TestHooks', [Reflection.BindingFlags]'Public,NonPublic')
Assert-True ($null -ne $plannerHooksType) 'LocalG2WindowPlanner must expose narrow deterministic TestHooks.'
$planScenario = $plannerHooksType.GetMethod('Execute', [Reflection.BindingFlags]'Public,Static')

$separated = $planScenario.Invoke($null, @('SeparatedByOneMeter'))
Assert-Equal 2 $separated.RegionCount 'Events 1.0 m apart cannot share a 0.80 m total window.'
Assert-Equal '1,1' $separated.TransitionCounts 'Separated events must remain one event per region.'

$mergeable = $planScenario.Invoke($null, @('Mergeable'))
Assert-Equal 1 $mergeable.RegionCount 'Events covered by one legal total window must merge.'
Assert-Equal '2' $mergeable.TransitionCounts 'The merged region must retain both events.'

$partition = $planScenario.Invoke($null, @('ThreeEventPartition'))
Assert-Equal 2 $partition.RegionCount 'Three events must split at the first infeasible joint window.'
Assert-Equal '2,1' $partition.TransitionCounts 'Only a feasible consecutive subgroup may merge.'

$boundary = $planScenario.Invoke($null, @('NearBoundary'))
Assert-True ($boundary.FirstRightLength -gt $boundary.FirstLeftLength) `
    'A boundary-clamped total window must transfer missing length to the available side.'
Assert-True ($boundary.MaximumWindowLength -le 0.80 + 1e-9) `
    'No candidate window may exceed 0.80 m total length.'

foreach ($snapshot in @($separated, $mergeable, $partition, $boundary)) {
    Assert-True $snapshot.ExactEnvelope 'Region envelope must equal the extrema of actual legal variants.'
    Assert-True ($snapshot.MaximumWindowLength -le 0.80 + 1e-9) `
        'MaximumWindowLengthMeters is a total, not a per-side length.'
}

$repeat = $planScenario.Invoke($null, @('ThreeEventPartition'))
Assert-Equal $partition.Signature $repeat.Signature `
    'Repeated planning must preserve grouping, candidate numbering and variant order.'
  • Step 3: Run the detection script to verify RED

Run:

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

Expected: fail because the current event-±0.80 m ranges merge the SeparatedByOneMeter scenario.

  • Step 4: Replace overlap grouping with candidate-feasibility grouping

Replace the while (cursor < ordered.Count) body in TryPlan with:

while (cursor < ordered.Count)
{
    CurvatureTransition first = ordered[cursor];
    double segmentLength = segmentLengths[first.SegmentIndex];
    var group = new List<CurvatureTransition> { first };
    IReadOnlyList<LocalG2WindowVariant> variants =
        BuildVariants(group, segmentLength, options);
    if (variants.Count == 0)
    {
        reason = "局部 G2 单事件无法生成满足总长度约束的窗口。";
        return false;
    }
    cursor++;

    while (cursor < ordered.Count &&
        ordered[cursor].SegmentIndex == first.SegmentIndex)
    {
        var tentative = new List<CurvatureTransition>(group)
        {
            ordered[cursor],
        };
        IReadOnlyList<LocalG2WindowVariant> tentativeVariants =
            BuildVariants(tentative, segmentLength, options);
        if (tentativeVariants.Count == 0) break;
        group = tentative;
        variants = tentativeVariants;
        cursor++;
    }

    double minimumStart = double.PositiveInfinity;
    double maximumEnd = double.NegativeInfinity;
    for (int variantIndex = 0; variantIndex < variants.Count; variantIndex++)
    {
        minimumStart = Math.Min(
            minimumStart,
            variants[variantIndex].StartArcLengthMeters);
        maximumEnd = Math.Max(
            maximumEnd,
            variants[variantIndex].EndArcLengthMeters);
    }

    planned.Add(new LocalG2SmoothingRegion(
        first.SegmentIndex,
        group,
        minimumStart,
        maximumEnd,
        variants));
}

Delete MaximumLegalRange and WindowRange; they encode the erroneous per-side interpretation.

At the start of BuildTargets, add:

if (segmentLength + MergeToleranceMeters < options.MinimumWindowLengthMeters)
    return new ReadOnlyCollection<double>(new List<double>());

In AddIfLegal, immediately before constructing the variant, add:

double actualLength = end - start;
if (actualLength + MergeToleranceMeters < options.MinimumWindowLengthMeters ||
    actualLength > options.MaximumWindowLengthMeters + MergeToleranceMeters)
{
    return;
}

Keep candidateIndex = variants.Count, so numbering remains deterministic after infeasible variants are skipped.

  • Step 5: Strengthen the immutable region invariant

In the LocalG2SmoothingRegion constructor, reject non-finite envelopes and an empty variant list. Add using MultiWheelC.TrajectoryPlanning.Utils; and use this .NET Standard 2.0-compatible code:

if (segmentIndex < 0 || transitions == null || transitions.Count == 0 ||
    !NumericGuard.IsFinite(maximumStartArcLengthMeters) ||
    !NumericGuard.IsFinite(maximumEndArcLengthMeters) ||
    maximumStartArcLengthMeters < 0d ||
    maximumEndArcLengthMeters < maximumStartArcLengthMeters ||
    windowVariants == null || windowVariants.Count == 0)
{
    throw new ArgumentOutOfRangeException(nameof(transitions));
}
  • Step 6: Run focused and downstream tests

Run:

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_local_g2_candidates.ps1
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_local_g2_curve.ps1

Expected: all scripts pass.

  • Step 7: Commit the window fix
git add -- `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2WindowPlanner.cs `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2SmoothingRegion.cs `
  ClumsyPilot/tests/verify_path_smoothing_local_g2_detection.ps1
git diff --cached --check
git commit -m "fix: enforce Local G2 total window length"

Task 3: Add Deterministic Region Work Order and Two-Window Proof

Files:

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

Interfaces:

  • Consumes: report-order IReadOnlyList<LocalG2SmoothingRegion>.
  • Produces:
internal bool TryCreate(
    IReadOnlyList<LocalG2SmoothingRegion> reportOrder,
    out IReadOnlyList<LocalG2SmoothingRegion> workOrder,
    out string reason);

  • Ordering: segment ascending; inside one segment, first transition local arc descending; exact ties retain original report index.

  • The method returns a new read-only collection and never mutates reportOrder.

  • Step 1: Add the failing two-region candidate scenario

Extend the LocalG2CandidateBuilder.TestHooks.Execute switch:

case "TwoRegionWorkOrder": return BuildTwoRegionWorkOrder();

Add these optional constructor parameters and public properties to CandidateTestSnapshot:

bool workOrderDescending = false,
bool frontArcPreservedAfterBackReplacement = false,
bool bothReplacementsRetained = false,
bool forwardOrderRejected = false,
bool deterministicWorkOrder = false,
bool invalidWorkOrderRejected = false

WorkOrderDescending = workOrderDescending;
FrontArcPreservedAfterBackReplacement = frontArcPreservedAfterBackReplacement;
BothReplacementsRetained = bothReplacementsRetained;
ForwardOrderRejected = forwardOrderRejected;
DeterministicWorkOrder = deterministicWorkOrder;
InvalidWorkOrderRejected = invalidWorkOrderRejected;

public bool WorkOrderDescending { get; }
public bool FrontArcPreservedAfterBackReplacement { get; }
public bool BothReplacementsRetained { get; }
public bool ForwardOrderRejected { get; }
public bool DeterministicWorkOrder { get; }
public bool InvalidWorkOrderRejected { get; }

Add this scenario method inside TestHooks:

private static CandidateTestSnapshot BuildTwoRegionWorkOrder()
{
    var points = new List<SmoothingPoint2D>();
    for (int index = 0; index <= 12; index++)
    {
        double arc = index * 0.25d;
        points.Add(new SmoothingPoint2D(
            arc,
            0d,
            arc,
            0d,
            0d,
            1d,
            false,
            SmoothedPathPointSource.Anchor));
    }

    var segment = new PreparedDirectionSegment(
        0,
        TravelDirection.Forward,
        points,
        false,
        false);
    var original = new PreparedPath(new[] { segment });
    LocalG2SmoothingRegion frontRegion = CreateOrderedRegion(0.75d, 0.5d, 1.0d, 0);
    LocalG2SmoothingRegion backRegion = CreateOrderedRegion(2.25d, 2.0d, 2.5d, 1);
    var reportOrder = new[] { frontRegion, backRegion };

    var orderer = new LocalG2RegionWorkOrder();
    if (!orderer.TryCreate(
            reportOrder,
            out IReadOnlyList<LocalG2SmoothingRegion> workOrder,
            out string orderReason))
    {
        throw new InvalidOperationException(orderReason);
    }
    if (!orderer.TryCreate(
            reportOrder,
            out IReadOnlyList<LocalG2SmoothingRegion> repeatedOrder,
            out string repeatedReason))
    {
        throw new InvalidOperationException(repeatedReason);
    }

    bool descending =
        ReferenceEquals(backRegion, workOrder[0]) &&
        ReferenceEquals(frontRegion, workOrder[1]);
    bool deterministic =
        ReferenceEquals(workOrder[0], repeatedOrder[0]) &&
        ReferenceEquals(workOrder[1], repeatedOrder[1]) &&
        ReferenceEquals(frontRegion, reportOrder[0]) &&
        ReferenceEquals(backRegion, reportOrder[1]);

    var wrongSegmentTransition = new CurvatureTransition(
        1,
        0,
        1,
        0.75d,
        0.75d,
        0d,
        0d,
        0d,
        0.4d);
    var invalidRegion = new LocalG2SmoothingRegion(
        0,
        new[] { wrongSegmentTransition },
        0.5d,
        1.0d,
        new[]
        {
            new LocalG2WindowVariant(0, 0.5d, 1.0d, 0.25d, 0.25d),
        });
    bool invalidWorkOrderRejected =
        !orderer.TryCreate(
            new[] { invalidRegion },
            out _,
            out _) &&
        !orderer.TryCreate(
            new LocalG2SmoothingRegion[] { null },
            out _,
            out _);

    LocalG2CandidateGeometry frontCandidate =
        CreateLengthChangingCandidate(0, 0.5d, 1.0d);
    LocalG2CandidateGeometry backCandidate =
        CreateLengthChangingCandidate(1, 2.0d, 2.5d);
    var splicer = new LocalG2PathSplicer();

    if (!splicer.TryReplace(
            original,
            backCandidate,
            out PreparedPath afterBack,
            out string backReason))
    {
        throw new InvalidOperationException(backReason);
    }

    bool frontArcPreserved =
        PathReferenceInterpolator.TryInterpolateByArcLength(
            afterBack.Segments[0].Points,
            0.5d,
            out SmoothingPoint2D frontStart,
            out _) &&
        PathReferenceInterpolator.TryInterpolateByArcLength(
            afterBack.Segments[0].Points,
            1.0d,
            out SmoothingPoint2D frontEnd,
            out _) &&
        Math.Abs(frontStart.X - 0.5d) <= 1e-12d &&
        Math.Abs(frontEnd.X - 1.0d) <= 1e-12d;

    if (!splicer.TryReplace(
            afterBack,
            frontCandidate,
            out PreparedPath afterBoth,
            out string frontReason))
    {
        throw new InvalidOperationException(frontReason);
    }

    int localG2PointCount = 0;
    for (int index = 0; index < afterBoth.Segments[0].Points.Count; index++)
    {
        if (afterBoth.Segments[0].Points[index].Source ==
            SmoothedPathPointSource.LocalG2Transition)
        {
            localG2PointCount++;
        }
    }

    if (!splicer.TryReplace(
            original,
            frontCandidate,
            out PreparedPath afterFront,
            out string firstReason))
    {
        throw new InvalidOperationException(firstReason);
    }
    bool forwardOrderRejected = !splicer.TryReplace(
        afterFront,
        backCandidate,
        out _,
        out _);

    return new CandidateTestSnapshot(
        0,
        0d,
        0d,
        0d,
        0d,
        false,
        0,
        false,
        string.Empty,
        false,
        false,
        false,
        false,
        false,
        false,
        false,
        descending,
        frontArcPreserved,
        localG2PointCount >= 2,
        forwardOrderRejected,
        deterministic,
        invalidWorkOrderRejected);
}

private static LocalG2SmoothingRegion CreateOrderedRegion(
    double eventArc,
    double startArc,
    double endArc,
    int index)
{
    var transition = new CurvatureTransition(
        0,
        index,
        index + 1,
        eventArc,
        eventArc,
        0d,
        0d,
        0d,
        0.4d);
    return new LocalG2SmoothingRegion(
        0,
        new[] { transition },
        startArc,
        endArc,
        new[]
        {
            new LocalG2WindowVariant(
                0,
                startArc,
                endArc,
                eventArc - startArc,
                endArc - eventArc),
        });
}

private static LocalG2CandidateGeometry CreateLengthChangingCandidate(
    int candidateIndex,
    double startArc,
    double endArc)
{
    double middleArc = 0.5d * (startArc + endArc);
    var points = new[]
    {
        Point(startArc, 0d, startArc, false),
        Point(middleArc, 0.20d, middleArc, false),
        Point(endArc, 0d, endArc, false),
    };
    return new LocalG2CandidateGeometry(
        candidateIndex,
        0,
        startArc,
        endArc,
        middleArc - startArc,
        endArc - middleArc,
        points,
        0d,
        0d,
        0d,
        0d,
        true);
}

Append to verify_path_smoothing_local_g2_candidates.ps1:

$twoRegions = Invoke-Scenario 'TwoRegionWorkOrder'
Assert-True $twoRegions.WorkOrderDescending `
    'Same-segment regions must be processed from larger original local arc to smaller local arc.'
Assert-True $twoRegions.FrontArcPreservedAfterBackReplacement `
    'Replacing the back region must preserve the front region original arc coordinates.'
Assert-True $twoRegions.BothReplacementsRetained `
    'Back-then-front replacement must retain both length-changing local replacements.'
Assert-True $twoRegions.ForwardOrderRejected `
    'The regression fixture must prove that front-first invalidates the original back absolute arc.'
Assert-True $twoRegions.DeterministicWorkOrder `
    'Work ordering must repeat exactly without mutating report order.'
Assert-True $twoRegions.InvalidWorkOrderRejected `
    'Work ordering must reject null regions and cross-segment event contents.'
  • Step 2: Run the candidate script to verify RED

Run:

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

Expected: build fails because LocalG2RegionWorkOrder does not exist.

  • Step 3: Create the work-order component

Create ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2RegionWorkOrder.cs with:

using System;
using System.Collections.Generic;
using System.Collections.ObjectModel;
using MultiWheelC.TrajectoryPlanning.Utils;

namespace MultiWheelC.TrajectoryPlanning.PathSmoothing.LocalG2;

/// <summary>把稳定报告顺序转换为不会使后续原始局部弧长错位的工作顺序。</summary>
internal sealed class LocalG2RegionWorkOrder
{
    internal bool TryCreate(
        IReadOnlyList<LocalG2SmoothingRegion> reportOrder,
        out IReadOnlyList<LocalG2SmoothingRegion> workOrder,
        out string reason)
    {
        workOrder = Empty();
        reason = string.Empty;
        if (reportOrder == null)
        {
            reason = "局部 G2 区域工作顺序输入无效。";
            return false;
        }

        var indexed = new List<IndexedRegion>(reportOrder.Count);
        for (int index = 0; index < reportOrder.Count; index++)
        {
            LocalG2SmoothingRegion region = reportOrder[index];
            if (!TryValidate(region, out double firstArc, out reason))
                return false;
            indexed.Add(new IndexedRegion(region, index, firstArc));
        }

        indexed.Sort(Compare);
        var ordered = new List<LocalG2SmoothingRegion>(indexed.Count);
        for (int index = 0; index < indexed.Count; index++)
            ordered.Add(indexed[index].Region);
        workOrder = new ReadOnlyCollection<LocalG2SmoothingRegion>(ordered);
        return true;
    }

    private static bool TryValidate(
        LocalG2SmoothingRegion region,
        out double firstArc,
        out string reason)
    {
        firstArc = 0d;
        reason = string.Empty;
        if (region == null || region.SegmentIndex < 0 ||
            region.Transitions == null || region.Transitions.Count == 0 ||
            region.WindowVariants == null || region.WindowVariants.Count == 0)
        {
            reason = "局部 G2 工作顺序包含空区域或空窗口集。";
            return false;
        }

        double previousArc = -1d;
        for (int index = 0; index < region.Transitions.Count; index++)
        {
            CurvatureTransition transition = region.Transitions[index];
            if (transition == null ||
                transition.SegmentIndex != region.SegmentIndex ||
                !NumericGuard.IsFinite(transition.LocalArcLengthMeters) ||
                transition.LocalArcLengthMeters < previousArc)
            {
                reason = "局部 G2 工作顺序要求区域事件有限、同段且按弧长升序。";
                return false;
            }
            previousArc = transition.LocalArcLengthMeters;
        }

        firstArc = region.Transitions[0].LocalArcLengthMeters;
        return true;
    }

    private static int Compare(IndexedRegion left, IndexedRegion right)
    {
        int segment = left.Region.SegmentIndex.CompareTo(right.Region.SegmentIndex);
        if (segment != 0) return segment;
        int descendingArc = right.FirstArc.CompareTo(left.FirstArc);
        return descendingArc != 0
            ? descendingArc
            : left.ReportIndex.CompareTo(right.ReportIndex);
    }

    private static IReadOnlyList<LocalG2SmoothingRegion> Empty()
    {
        return new ReadOnlyCollection<LocalG2SmoothingRegion>(
            new List<LocalG2SmoothingRegion>());
    }

    private sealed class IndexedRegion
    {
        internal IndexedRegion(
            LocalG2SmoothingRegion region,
            int reportIndex,
            double firstArc)
        {
            Region = region;
            ReportIndex = reportIndex;
            FirstArc = firstArc;
        }

        internal LocalG2SmoothingRegion Region { get; }
        internal int ReportIndex { get; }
        internal double FirstArc { get; }
    }
}
  • Step 4: Run focused determinism and splicing tests

Run twice:

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_local_g2_candidates.ps1
powershell -ExecutionPolicy Bypass -File ClumsyPilot/tests/verify_path_smoothing_local_g2_detection.ps1

Expected: every command passes; both candidate runs produce the same work-order and coordinate assertions.

  • Step 5: Commit the work-order component
git add -- `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2RegionWorkOrder.cs `
  ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2CandidateBuilder.cs `
  ClumsyPilot/tests/verify_path_smoothing_local_g2_candidates.ps1
git diff --cached --check
git commit -m "fix: order Local G2 regions back to front"

Task 4: Make Task 8 Consume the Stabilized Contracts

Files:

  • Modify: docs/superpowers/plans/2026-07-30-local-g2-path-presmoothing.md

Interfaces:

  • Consumes:
LocalG2RegionWorkOrder.TryCreate(
    IReadOnlyList<LocalG2SmoothingRegion> reportOrder,
    out IReadOnlyList<LocalG2SmoothingRegion> workOrder,
    out string reason);

  • Produces: an updated Task 8 plan in which processing order and report order are separate, and a real two-region pipeline test is mandatory.

  • Step 1: Add the work-order dependency to Task 8

In Task 8 Files, add:

- Consume unchanged: `ClumsyPilot/ParkrobTrajplanner/PathSmoothing/LocalG2/LocalG2RegionWorkOrder.cs`

In Task 8 Interfaces, add:

- Consumes `LocalG2RegionWorkOrder.TryCreate(...)`; pipeline iteration must not use report-order regions directly.
- Publishes region reports in the detector's original ascending order, independently of processing order.
  • Step 2: Replace Task 8 sequential-processing pseudocode

Replace the foreach (LocalG2SmoothingRegion region in regions) block with:

if (!_workOrder.TryCreate(
        regions,
        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>(regions.Count);
for (int reportIndex = 0; reportIndex < regions.Count; reportIndex++)
    reports.Add(reportsByRegion[regions[reportIndex]]);
  • Step 3: Add a real two-region Task 8 acceptance case

Add this exact requirement to Task 8 Step 1:

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`.
  • Step 4: Check that Task 8 no longer documents unsafe ascending iteration

Run:

rg -n "foreach \\(LocalG2SmoothingRegion region in regions\\)|LocalG2RegionWorkOrder|workRegions|reportsByRegion|two disjoint detected" `
  docs/superpowers/plans/2026-07-30-local-g2-path-presmoothing.md

Expected:

  • no match for foreach (LocalG2SmoothingRegion region in regions);

  • matches for LocalG2RegionWorkOrder, workRegions, reportsByRegion, and the two-region acceptance case.

  • Step 5: Commit the Task 8 gate update

git add -- docs/superpowers/plans/2026-07-30-local-g2-path-presmoothing.md
git diff --cached --check
git commit -m "docs: gate Local G2 pipeline on stable region order"

Task 5: Run the Complete Pre-Task-8 Verification Gate

Files:

  • Verify: ClumsyPilot/ClumsyPilot.csproj
  • Verify: all ClumsyPilot/tests/verify_path_smoothing_*.ps1
  • Verify: files committed by Tasks 1–4

Interfaces:

  • Consumes: the committed P0/P1 fixes and existing regression scripts.

  • Produces: evidence that the repository is ready to enter Task 8, without claiming that the Local G2 service pipeline exists.

  • Step 1: Build from the current committed source

Run:

dotnet build ClumsyPilot/ClumsyPilot.csproj --no-restore

Expected: build succeeds with zero errors. Existing obsolete API warnings may remain only if they were already present before this plan.

  • Step 2: Run every PathSmoothing verification script

Run:

$tests = @(
  'verify_path_smoothing_algorithm_input.ps1',
  'verify_path_smoothing_bezier.ps1',
  'verify_path_smoothing_bspline.ps1',
  'verify_path_smoothing_comparison.ps1',
  'verify_path_smoothing_contracts.ps1',
  'verify_path_smoothing_documentation.ps1',
  'verify_path_smoothing_fixtures.ps1',
  'verify_path_smoothing_geometry.ps1',
  'verify_path_smoothing_integration.ps1',
  'verify_path_smoothing_local_g2_candidates.ps1',
  'verify_path_smoothing_local_g2_curve.ps1',
  'verify_path_smoothing_local_g2_detection.ps1',
  'verify_path_smoothing_png.ps1',
  'verify_path_smoothing_quintic.ps1',
  'verify_path_smoothing_runner.ps1',
  'verify_path_smoothing_service.ps1',
  'verify_path_smoothing_svg_csv.ps1',
  'verify_path_smoothing_validation.ps1'
)
foreach ($test in $tests) {
    & powershell -ExecutionPolicy Bypass -File (Join-Path 'ClumsyPilot/tests' $test)
    if ($LASTEXITCODE -ne 0) { throw "$test failed with exit code $LASTEXITCODE" }
}

Expected: all scripts pass. In particular:

  • exact-limit forward/reverse circle checks pass at endpoints and interiors;

  • actually over-limit curvature remains rejected;

  • RectangleDetour raw baseline is Success;

  • 0.80 m is never treated as a per-side allowance;

  • the two-window reverse-order proof passes twice.

  • Step 3: Audit scope and staged state

Run:

git diff --check
git diff --cached --check
git status --short
git log -4 --oneline

Expected:

  • no whitespace errors;

  • no staged files remain;

  • the four task commits are visible;

  • unrelated pre-existing worktree changes remain untouched;

  • no LocalG2PreSmoothingPipeline.cs exists and PathSmoothingService.cs was not changed by this plan.

  • Step 4: Record the completion boundary

Report exactly:

任务 8 前稳定化完成：P0 曲率门、0.80 m 总窗口语义和多区域逆序底层证明均已通过。
当前只具备进入任务 8 的条件；LocalG2PreSmoothingPipeline 和正式服务分派仍未实现。

Do not create an empty verification commit.

Reference in New Issue View Git Blame Copy Permalink