# EM Longitudinal Rolling Planning 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:** 修复 EM 纵向 ST 规划，使局部空间窗口能够连续滚动，只在真实 Goal 或 GearSwitchApproach 边界以 jerk 受限的 `S=S_end、U=0、A=0` 状态停车。

**Architecture:** 将 LS 空间窗口、ST 时间范围和真实停车边界拆成独立概念，由 `EmLongitudinalMode` 统一驱动速度包络、QP终端约束、初始迭代、轨迹采样和发布验证。使用与 QP 恒 jerk 动力学一致的完整 S 曲线停车模型，并将上一条已发布轨迹按绝对时间重采样为纵向软参考。

**Tech Stack:** C# 10、.NET Standard 2.0、OSQP QP接口、`EMPlannerVerificationHost`（.NET 10 Windows 控制台验证宿主）、PowerShell、Git。

## Global Constraints

- 设计规格以 `docs/superpowers/specs/2026-08-05-em-longitudinal-rolling-planning-design.md` 为唯一行为依据。
- `DistanceHorizonMeters` 是 LS 空间前视范围，不是 ST 必须在本周期走完的距离。
- `TimeHorizonSeconds` 是 ST 优化时间范围，不包含 `ZeroSpeedHoldSeconds`。
- 只有真实 `Goal` 或 `GearSwitchApproach` 可以触发准确停车；局部窗口截断点不能触发零速保持。
- 准确停车必须在 ST 时间范围结束前到达 `S=S_end、U=0、A=0`，并在 QP 内保留至少一个完整 `OutputTimeStepSeconds` 的静止稳定尾；不得放宽速度、加速度、jerk、QP残差或发布验证阈值。
- 滚动和接近模式允许非零末点速度、加速度及 yaw rate，但仍必须满足所有普通运动学和碰撞限制。
- `OBSERVE_ONLY` 保持不变；不增加底盘命令、不增加轨迹执行器超时看门狗。
- 保留工作树中与本计划无关的用户修改；每次只暂存当前任务列出的文件。

---

## File Structure

### 新建文件

- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmLongitudinalMode.cs`：定义滚动、接近停车边界、准确停车三种纵向行为。
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/JerkLimitedStoppingMath.cs`：完整 jerk 受限停车曲线、停止距离反求和时间范围内最大可停车行驶距离。
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalTerminalSchedule.cs`：统一计算准确停车锚点和 QP 内部稳定尾 knot 范围。
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPreviousTrajectorySeedBuilder.cs`：将上一条世界坐标轨迹按新时间 knot 重采样并映射到新 LateralPath 的 PathS。

### 主要修改文件

- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/PlanningHorizonSelector.cs`：空间窗口和停车可达性选择。
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanningInput.cs`：拆分路径上界、真实停车边界和纵向模式。
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimit.cs`：允许滚动路径末端非零速度上限。
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs`：按模式构建 jerk 受限停车速度包络。
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs`：仅准确停车添加 `S/U/A` 终端硬约束。
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs`：按模式验证终端状态及逐点可停车域。
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs`：按模式生成初始迭代并取消滚动末点强制固定。
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectoryMetadata.cs`：在发布轨迹中记录纵向模式。
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs`：只为准确停车追加零速保持点。
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs`：只为准确停车标记真实边界锚点。
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmTrajectoryValidator.cs`：条件化终端验证并新增终点加速度校验。
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmPlanningRequestValidator.cs`：验证空间前视的停车余量。
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs`：传递纵向模式、上一轨迹种子和模式诊断。
- `ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs`：显示模式、停车能力和成功末态。
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md`：记录新的窗口和终端语义。

### 测试文件

- `ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs`
- `ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs`
- `ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs`
- `ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs`
- `ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs`

---

### Task 1: 完整 jerk 受限停车数学

**Files:**
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/JerkLimitedStoppingMath.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs`
- Test: `ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs`

**Interfaces:**
- Produces: `JerkLimitedStoppingMath.TryCalculate(double, double, double, double, out JerkLimitedStoppingProfile, out string)`、`MaximumInitialSpeedForDistance(double, double, double, double, double)`、`CalculateMaximumStoppedDistance(double, double, double, double, double, double, double)`。
- Produces: `JerkLimitedStoppingProfile`，包含距离、持续时间、末速和末加速度。
- Consumers: Tasks 2、3、4、8。

- [ ] **Step 1: 写出停车末态和可达距离失败测试**

在 `LongitudinalModelChecks.Run()` 开头加入：

```csharp
VerifiesJerkLimitedStoppingProfileEndsAtRest();
VerifiesStoppedReachabilityUsesTheSameJerkModel();
```

新增完整测试：

```csharp
private static void VerifiesJerkLimitedStoppingProfileEndsAtRest()
{
    Verification.True(JerkLimitedStoppingMath.TryCalculate(
        0.20d, 0d, 0.30d, 0.50d,
        out JerkLimitedStoppingProfile profile, out string failure),
        "jerk-limited stop builds: " + failure);
    Verification.True(profile.DistanceMeters > 0d, "stop distance is positive");
    Verification.True(profile.DurationSeconds > 0d, "stop duration is positive");
    Verification.NearlyEqual(0d, profile.FinalSpeedMetersPerSecond,
        "stop ends at zero speed");
    Verification.NearlyEqual(0d, profile.FinalAccelerationMetersPerSecondSquared,
        "stop releases acceleration to zero");

    Verification.True(JerkLimitedStoppingMath.TryCalculate(
        0.20d, 0.20d, 0.30d, 0.50d,
        out JerkLimitedStoppingProfile accelerating, out failure),
        "positive-acceleration stop builds: " + failure);
    Verification.True(accelerating.DistanceMeters > profile.DistanceMeters,
        "positive initial acceleration needs more stopping distance");
    Verification.NearlyEqual(0d, accelerating.FinalAccelerationMetersPerSecondSquared,
        "positive-acceleration stop also releases acceleration");
}

private static void VerifiesStoppedReachabilityUsesTheSameJerkModel()
{
    double maximumDistance = JerkLimitedStoppingMath.CalculateMaximumStoppedDistance(
        0d, 0d, 0.20d, 0.20d, 0.30d, 0.50d, 2d);
    Verification.True(maximumDistance > 0d && maximumDistance < 0.40d,
        "two-second stopped reach is finite and below unconstrained cruise distance");

    double cap = JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(
        maximumDistance, 0.20d, 0.30d, 0.50d, 0.20d);
    Verification.True(cap >= 0d && cap <= 0.20d,
        "distance inversion stays inside the direction speed range");
}
```

- [ ] **Step 2: 运行测试并确认因类型不存在而失败**

Run:

```powershell
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model
```

Expected: 编译失败，指出 `JerkLimitedStoppingMath` 和 `JerkLimitedStoppingProfile` 不存在。

- [ ] **Step 3: 新建停车数学并迁移旧类型**

在新文件定义：

```csharp
public sealed class JerkLimitedStoppingProfile
{
    internal JerkLimitedStoppingProfile(double distanceMeters, double durationSeconds,
        double finalSpeedMetersPerSecond, double finalAccelerationMetersPerSecondSquared)
    {
        DistanceMeters = distanceMeters;
        DurationSeconds = durationSeconds;
        FinalSpeedMetersPerSecond = finalSpeedMetersPerSecond;
        FinalAccelerationMetersPerSecondSquared = finalAccelerationMetersPerSecondSquared;
    }

    public double DistanceMeters { get; }
    public double DurationSeconds { get; }
    public double FinalSpeedMetersPerSecond { get; }
    public double FinalAccelerationMetersPerSecondSquared { get; }
}

public static class JerkLimitedStoppingMath
{
    private const double NumericTolerance = 1e-12d;

    public static bool TryCalculate(double speed, double acceleration,
        double maximumDeceleration, double maximumJerk,
        out JerkLimitedStoppingProfile profile, out string failureReason)
    {
        profile = null;
        failureReason = string.Empty;
        if (!IsFinite(speed) || speed < 0d || !IsFinite(acceleration) ||
            !IsPositiveFinite(maximumDeceleration) || !IsPositiveFinite(maximumJerk) ||
            acceleration < -maximumDeceleration - NumericTolerance)
        {
            failureReason = "Stopping inputs are outside finite longitudinal bounds.";
            return false;
        }
        if (speed <= NumericTolerance && Math.Abs(acceleration) <= NumericTolerance)
        {
            profile = new JerkLimitedStoppingProfile(0d, 0d, 0d, 0d);
            return true;
        }

        double unavoidableReleaseLoss = acceleration < 0d
            ? acceleration * acceleration / (2d * maximumJerk)
            : 0d;
        if (speed + NumericTolerance < unavoidableReleaseLoss)
        {
            failureReason = "The current negative acceleration cannot be released before speed crosses zero.";
            return false;
        }

        double peakDeceleration = Math.Sqrt(maximumJerk * speed +
            0.5d * acceleration * acceleration);
        double downDuration;
        double plateauDuration;
        double upDuration;
        if (peakDeceleration <= maximumDeceleration + NumericTolerance)
        {
            peakDeceleration = Math.Min(peakDeceleration, maximumDeceleration);
            downDuration = (acceleration + peakDeceleration) / maximumJerk;
            plateauDuration = 0d;
            upDuration = peakDeceleration / maximumJerk;
        }
        else
        {
            peakDeceleration = maximumDeceleration;
            downDuration = (acceleration + peakDeceleration) / maximumJerk;
            upDuration = peakDeceleration / maximumJerk;
            double speedAfterDown = speed + acceleration * downDuration -
                0.5d * maximumJerk * downDuration * downDuration;
            double releaseLoss = peakDeceleration * peakDeceleration /
                (2d * maximumJerk);
            plateauDuration = (speedAfterDown - releaseLoss) / peakDeceleration;
        }
        if (downDuration < -NumericTolerance || plateauDuration < -NumericTolerance)
        {
            failureReason = "No monotone three-phase jerk-limited stop exists for the current state.";
            return false;
        }

        downDuration = Math.Max(0d, downDuration);
        plateauDuration = Math.Max(0d, plateauDuration);
        double s = 0d;
        double u = speed;
        double a = acceleration;
        Integrate(ref s, ref u, ref a, -maximumJerk, downDuration);
        Integrate(ref s, ref u, ref a, 0d, plateauDuration);
        Integrate(ref s, ref u, ref a, maximumJerk, upDuration);
        if (Math.Abs(u) > 1e-9d || Math.Abs(a) > 1e-9d || s < -NumericTolerance)
        {
            failureReason = "The jerk-limited stop did not end at zero speed and acceleration.";
            return false;
        }
        profile = new JerkLimitedStoppingProfile(Math.Max(0d, s),
            downDuration + plateauDuration + upDuration, 0d, 0d);
        return true;
    }

    public static double MaximumInitialSpeedForDistance(double availableDistance,
        double conservativeInitialAcceleration, double maximumDeceleration,
        double maximumJerk, double directionMaximumSpeed)
    {
        if (!IsFinite(availableDistance) || availableDistance < 0d ||
            !IsFinite(conservativeInitialAcceleration) ||
            !IsPositiveFinite(directionMaximumSpeed))
            throw new ArgumentOutOfRangeException(nameof(availableDistance));
        double lower = 0d;
        double upper = directionMaximumSpeed;
        for (int iteration = 0; iteration < 64; iteration++)
        {
            double candidate = 0.5d * (lower + upper);
            bool fits = TryCalculate(candidate, conservativeInitialAcceleration,
                maximumDeceleration, maximumJerk,
                out JerkLimitedStoppingProfile stop, out _) &&
                stop.DistanceMeters <= availableDistance + NumericTolerance;
            if (fits) lower = candidate;
            else upper = candidate;
        }
        return lower;
    }

    public static double CalculateMaximumStoppedDistance(double initialSpeed,
        double initialAcceleration, double maximumSpeed, double maximumAcceleration,
        double maximumDeceleration, double maximumJerk, double timeHorizon)
    {
        if (!IsFinite(initialSpeed) || initialSpeed < 0d ||
            !IsFinite(initialAcceleration) || !IsPositiveFinite(maximumSpeed) ||
            !IsPositiveFinite(maximumAcceleration) ||
            !IsPositiveFinite(maximumDeceleration) ||
            !IsPositiveFinite(maximumJerk) || !IsPositiveFinite(timeHorizon))
            throw new ArgumentOutOfRangeException(nameof(timeHorizon));
        double lower = 0d;
        double upper = timeHorizon;
        double bestDistance = 0d;
        for (int iteration = 0; iteration < 64; iteration++)
        {
            double driveDuration = 0.5d * (lower + upper);
            AdvanceTowardMaximumSpeed(initialSpeed, initialAcceleration,
                maximumSpeed, maximumAcceleration, maximumJerk, driveDuration,
                out double driveDistance, out double speed, out double acceleration);
            bool fits = TryCalculate(speed, acceleration, maximumDeceleration,
                maximumJerk, out JerkLimitedStoppingProfile stop, out _) &&
                driveDuration + stop.DurationSeconds <= timeHorizon + NumericTolerance;
            if (fits)
            {
                lower = driveDuration;
                bestDistance = Math.Max(bestDistance, driveDistance + stop.DistanceMeters);
            }
            else
            {
                upper = driveDuration;
            }
        }
        return bestDistance;
    }

    private static void AdvanceTowardMaximumSpeed(double initialSpeed,
        double initialAcceleration, double maximumSpeed, double maximumAcceleration,
        double maximumJerk, double duration, out double distance,
        out double speed, out double acceleration)
    {
        distance = 0d;
        speed = initialSpeed;
        acceleration = initialAcceleration;
        double remaining = duration;
        while (remaining > NumericTolerance)
        {
            double dt = Math.Min(0.001d, remaining);
            double speedNeededToReleaseAcceleration = acceleration > 0d
                ? acceleration * acceleration / (2d * maximumJerk)
                : 0d;
            double jerk = speed + speedNeededToReleaseAcceleration >= maximumSpeed
                ? (acceleration > 0d ? -maximumJerk : 0d)
                : (acceleration < maximumAcceleration ? maximumJerk : 0d);
            Integrate(ref distance, ref speed, ref acceleration, jerk, dt);
            if (speed > maximumSpeed && speed - maximumSpeed <= 1e-6d)
            {
                speed = maximumSpeed;
                acceleration = 0d;
            }
            remaining -= dt;
        }
    }

    private static void Integrate(ref double s, ref double u, ref double a,
        double jerk, double duration)
    {
        s += u * duration + 0.5d * a * duration * duration +
            jerk * duration * duration * duration / 6d;
        u += a * duration + 0.5d * jerk * duration * duration;
        a += jerk * duration;
    }

    private static bool IsPositiveFinite(double value)
    {
        return IsFinite(value) && value > 0d;
    }

    private static bool IsFinite(double value)
    {
        return !double.IsNaN(value) && !double.IsInfinity(value);
    }
}
```

按上述方法实现；不保留 `PathSpeedLimitBuilder.cs` 文件末尾旧的 `LongitudinalStoppingProfile` 和 `LongitudinalStoppingMath`，避免同名实现并存。

- [ ] **Step 4: 运行纵向模型测试并确认通过**

Run:

```powershell
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model
```

Expected: `PASS longitudinal-model`。

- [ ] **Step 5: 独立提交停车数学**

```powershell
git add -- ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/JerkLimitedStoppingMath.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs
git commit -m "feat: add complete jerk-limited stopping math"
```

---

### Task 2: 纵向模式、空间窗口和输入契约

**Files:**
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmLongitudinalMode.cs`
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalTerminalSchedule.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/PlanningHorizonSelector.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanningInput.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmPlanningRequestValidator.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs`
- Test: `ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs`
- Test: `ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs`

**Interfaces:**
- Consumes: Task 1 `JerkLimitedStoppingMath`。
- Produces: `EmLongitudinalMode`、`LongitudinalTerminalSchedule.GetStabilizationStartIndex(IReadOnlyList<double>, double)`、扩展后的 `PlanningHorizonSelection`、扩展后的 `LongitudinalPlanningInput`。
- Consumers: Tasks 3–8。

- [ ] **Step 1: 写出三种模式和固定空间窗口测试**

替换 `VerifiesReferenceHorizonSelectionKeepsTheCurrentSegmentBoundary()` 的断言并拆成三个场景：

```csharp
private static void VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime()
{
    EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
    configuration.Scheduling.DistanceHorizonMeters = 1d;
    configuration.Scheduling.TimeHorizonSeconds = 2d;

    DirectionSegmentView longSegment = CreateSegment(4d, EmBoundaryType.Goal);
    EmPlanningStatus status = new PlanningHorizonSelector().Select(
        longSegment, 0d, 0d, 0d, configuration,
        out PlanningHorizonSelection rolling, out string failure);
    Verification.Equal(EmPlanningStatus.Success, status, "rolling selection: " + failure);
    Verification.NearlyEqual(1d, rolling.WindowEndReferenceS,
        "distance horizon defines the LS window");
    Verification.Equal(EmLongitudinalMode.RollingContinuation,
        rolling.LongitudinalMode, "far boundary rolls");

    DirectionSegmentView visibleButFar = CreateSegment(0.35d, EmBoundaryType.Goal);
    status = new PlanningHorizonSelector().Select(
        visibleButFar, 0d, 0d, 0d, configuration,
        out PlanningHorizonSelection approach, out failure);
    Verification.Equal(EmPlanningStatus.Success, status, "approach selection: " + failure);
    Verification.Equal(EmLongitudinalMode.ApproachStopBoundary,
        approach.LongitudinalMode, "visible unreachable boundary approaches");

    DirectionSegmentView reachableGoal = CreateSegment(0.10d, EmBoundaryType.Goal);
    status = new PlanningHorizonSelector().Select(
        reachableGoal, 0d, 0d, 0d, configuration,
        out PlanningHorizonSelection exact, out failure);
    Verification.Equal(EmPlanningStatus.Success, status, "exact selection: " + failure);
    Verification.Equal(EmLongitudinalMode.ExactStopAtBoundary,
        exact.LongitudinalMode, "reachable goal stops exactly");
    Verification.Equal(EmTerminalType.Goal, exact.TerminalType,
        "exact stop preserves Goal identity");

    IReadOnlyList<double> regularTimes = LongitudinalCandidate.CreateKnotTimes(2d, 0.1d);
    Verification.Equal(19, LongitudinalTerminalSchedule.GetStabilizationStartIndex(
        regularTimes, 0.1d), "regular exact stop reserves t=1.9..2.0");
    Verification.Equal(1, LongitudinalTerminalSchedule.GetStabilizationStartIndex(
        new[] { 0d, 0.1d, 0.2d, 0.25d }, 0.1d),
        "short final interval moves the stop anchor earlier");
}
```

在 `FoundationChecks` 添加配置失败场景，设置 `DistanceHorizonMeters` 小于“最大状态停车距离 + 一周期行驶余量”，断言 `InvalidInput` 且原因包含 `DistanceHorizonMeters`、`required` 和 `configured`。

- [ ] **Step 2: 运行失败测试**

Run:

```powershell
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- foundation
```

Expected: 编译失败，指出新枚举和选择结果属性不存在；旧选择器也不会返回三种预期模式。

- [ ] **Step 3: 增加模式和选择结果**

创建：

```csharp
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

public enum EmLongitudinalMode
{
    RollingContinuation,
    ApproachStopBoundary,
    ExactStopAtBoundary,
}
```

将 `PlanningHorizonSelection` 定义调整为：

```csharp
internal PlanningHorizonSelection(double windowEndReferenceS,
    EmTerminalType terminalType, EmLongitudinalMode longitudinalMode,
    double stopBoundaryReferenceS, bool hasStopBoundary)
{
    WindowEndReferenceS = windowEndReferenceS;
    TerminalType = terminalType;
    LongitudinalMode = longitudinalMode;
    StopBoundaryReferenceS = stopBoundaryReferenceS;
    HasStopBoundary = hasStopBoundary;
}

public double WindowEndReferenceS { get; }
public double TerminalReferenceS => WindowEndReferenceS;
public EmTerminalType TerminalType { get; }
public EmLongitudinalMode LongitudinalMode { get; }
public double StopBoundaryReferenceS { get; }
public bool HasStopBoundary { get; }
```

保留只读 `TerminalReferenceS` 兼容别名，避免同时修改不相关调用方；新代码只使用 `WindowEndReferenceS`。

同时创建统一稳定尾选择器：

```csharp
public static class LongitudinalTerminalSchedule
{
    public static int GetStabilizationStartIndex(IReadOnlyList<double> knotTimes,
        double minimumStabilizationDurationSeconds)
    {
        if (knotTimes == null || knotTimes.Count < 3 ||
            double.IsNaN(minimumStabilizationDurationSeconds) ||
            double.IsInfinity(minimumStabilizationDurationSeconds) ||
            minimumStabilizationDurationSeconds <= 0d)
            throw new ArgumentException("A positive stabilization tail and at least three knots are required.");
        double previous = double.NegativeInfinity;
        for (int index = 0; index < knotTimes.Count; index++)
        {
            if (double.IsNaN(knotTimes[index]) || double.IsInfinity(knotTimes[index]) ||
                knotTimes[index] <= previous)
                throw new ArgumentException("Terminal-schedule knots must be finite and strictly increasing.");
            previous = knotTimes[index];
        }
        double finalTime = knotTimes[knotTimes.Count - 1];
        for (int index = knotTimes.Count - 2; index >= 1; index--)
        {
            if (finalTime - knotTimes[index] >= minimumStabilizationDurationSeconds - 1e-12d)
                return index;
        }
        throw new ArgumentException("The time horizon cannot contain a full terminal stabilization interval.");
    }
}
```

- [ ] **Step 4: 重写窗口选择器**

核心分支必须等价于：

```csharp
double remaining = segment.LengthMeters - currentSegmentReferenceS;
if (!JerkLimitedStoppingMath.TryCalculate(initialSpeed, initialAcceleration,
        longitudinal.MaximumDecelerationMetersPerSecondSquared,
        longitudinal.MaximumJerkMetersPerSecondCubed,
        out JerkLimitedStoppingProfile stop, out failureReason))
    return EmPlanningStatus.StoppingDistanceInsufficient;

if (stop.DistanceMeters > remaining + BoundaryTolerance)
{
    failureReason = "StoppingDistanceInsufficient: remaining=" + remaining +
        ";required=" + stop.DistanceMeters +
        ";shortage=" + (stop.DistanceMeters - remaining);
    return EmPlanningStatus.StoppingDistanceInsufficient;
}

double windowEnd = Math.Min(currentSegmentReferenceS +
    scheduling.DistanceHorizonMeters, segment.LengthMeters);
bool containsBoundary = windowEnd >= segment.LengthMeters - BoundaryTolerance;
if (!containsBoundary)
{
    selection = new PlanningHorizonSelection(windowEnd,
        EmTerminalType.RollingSafetyStop,
        EmLongitudinalMode.RollingContinuation,
        segment.LengthMeters, false);
    return EmPlanningStatus.Success;
}

IReadOnlyList<double> knotTimes = LongitudinalCandidate.CreateKnotTimes(
    scheduling.TimeHorizonSeconds, scheduling.OutputTimeStepSeconds);
int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
    knotTimes, scheduling.OutputTimeStepSeconds);
double availableMotionTime = knotTimes[stabilizationStart];
double maximumStoppedDistance =
    JerkLimitedStoppingMath.CalculateMaximumStoppedDistance(
        initialSpeed, initialAcceleration, directionMaximum,
        longitudinal.MaximumAccelerationMetersPerSecondSquared,
        longitudinal.MaximumDecelerationMetersPerSecondSquared,
        longitudinal.MaximumJerkMetersPerSecondCubed,
        availableMotionTime);
EmLongitudinalMode mode = remaining <= maximumStoppedDistance + BoundaryTolerance
    ? EmLongitudinalMode.ExactStopAtBoundary
    : EmLongitudinalMode.ApproachStopBoundary;
selection = new PlanningHorizonSelection(segment.LengthMeters,
    ToTerminalType(segment.EndBoundary.BoundaryType), mode,
    segment.LengthMeters, true);
return EmPlanningStatus.Success;
```

删除旧 `CalculateReachableDistance()` 中扣除 `ZeroSpeedHoldSeconds` 的逻辑。

- [ ] **Step 5: 扩展纵向输入并验证组合**

给 `LongitudinalPlanningInput` 构造函数增加 `EmLongitudinalMode mode`，并增加：

```csharp
public EmLongitudinalMode Mode { get; }
public double PathUpperBoundS => Path.Points[Path.Points.Count - 1].PathS;
public bool HasStopBoundary => TerminalType != EmTerminalType.RollingSafetyStop;
public double StopBoundaryPathS => HasStopBoundary ? PathUpperBoundS : double.NaN;
[Obsolete("Use PathUpperBoundS or StopBoundaryPathS explicitly.")]
public double TerminalPathS => PathUpperBoundS;
```

构造函数拒绝以下矛盾组合：

```csharp
if (mode == EmLongitudinalMode.RollingContinuation &&
    terminalType != EmTerminalType.RollingSafetyStop)
    throw new ArgumentException("Rolling continuation requires a rolling window boundary.");
if (mode != EmLongitudinalMode.RollingContinuation &&
    terminalType == EmTerminalType.RollingSafetyStop)
    throw new ArgumentException("Stop-boundary modes require Goal or GearSwitch.");
```

- [ ] **Step 6: 增加前视停车余量配置验证**

在 `EmPlanningRequestValidator.TryValidateConfiguration` 中计算正向和反向最大速度下的保守停车距离，取较大值并增加：

```text
previewReserve = maxSpeed * ReplanPeriodSeconds
requiredDistanceHorizon = worstStoppingDistance + previewReserve
```

若配置不足，返回包含配置值和所需值的具体原因。配置验证还调用 `LongitudinalTerminalSchedule.GetStabilizationStartIndex`，要求预留一个完整 `OutputTimeStepSeconds` 稳定尾之后仍至少有一个正运动 interval；停车时间超过剩余运动时间由 `ApproachStopBoundary` 处理，不拒绝配置。

- [ ] **Step 7: 更新所有 `LongitudinalPlanningInput` 构造调用并运行测试**

所有现有 Goal/GearSwitch 测试先传 `EmLongitudinalMode.ExactStopAtBoundary`；原滚动测试传 `RollingContinuation`。`EmPlanningService` 创建纵向输入时传 `horizon.LongitudinalMode`，切片上界改用 `horizon.WindowEndReferenceS`。随后运行：

```powershell
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- foundation
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model
```

Expected: `PASS foundation` 和 `PASS longitudinal-model`。

- [ ] **Step 8: 独立提交模式与窗口语义**

```powershell
git add -- ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmLongitudinalMode.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalTerminalSchedule.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/PlanningHorizonSelector.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanningInput.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmPlanningRequestValidator.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs
git commit -m "feat: separate rolling horizons from stop boundaries"
```

---

### Task 3: 模式化 PathS 速度包络

**Files:**
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimit.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs`
- Test: `ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs`

**Interfaces:**
- Consumes: Task 1 停车距离反求；Task 2 `LongitudinalPlanningInput.Mode`。
- Produces: 末端可为非零的 `PathSpeedLimit`，以及面向真实边界的停止速度上限。
- Consumers: Tasks 4、5。

- [ ] **Step 1: 写出滚动末端非零和真实边界零速测试**

加入：

```csharp
private static void VerifiesRollingEnvelopeDoesNotStopAtWindowEnd()
{
    EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
    LateralPath path = CreatePath(new[]
    {
        new PathFixture(0d, 0d, 0d, 0d),
        new PathFixture(1d, 1d, 0d, 0d),
    });
    var rolling = new LongitudinalPlanningInput(path, TravelDirection.Forward,
        0d, 0d, EmTerminalType.RollingSafetyStop,
        EmLongitudinalMode.RollingContinuation, configuration,
        Array.Empty<double>(), Array.Empty<double>());
    EmPlanningStatus status = new PathSpeedLimitBuilder().Build(
        rolling, out PathSpeedLimit envelope, out string failure);
    Verification.Equal(EmPlanningStatus.Success, status, "rolling envelope: " + failure);
    Verification.True(envelope.MaximumSpeedAt(rolling.PathUpperBoundS) > 0d,
        "rolling window end keeps a nonzero speed allowance");
    Verification.True(!envelope.HasStopBoundary,
        "rolling envelope has no stop boundary");
}
```

保留 Goal 测试，但改为 `ExactStopAtBoundary` 并断言真实边界速度严格为零。增加 `ApproachStopBoundary` 测试，断言末端停止上限为零、时间末点不由该类固定。

- [ ] **Step 2: 运行测试并确认滚动末端仍被置零**

```powershell
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model
```

Expected: `rolling window end keeps a nonzero speed allowance` 失败。

- [ ] **Step 3: 修改 `PathSpeedLimit` 契约**

构造函数增加 `bool hasStopBoundary`，移除无条件末项为零校验，改为：

```csharp
if (hasStopBoundary &&
    (MaximumSpeedMetersPerSecond[MaximumSpeedMetersPerSecond.Count - 1] != 0d ||
     StoppingSpeedLimitsMetersPerSecond[StoppingSpeedLimitsMetersPerSecond.Count - 1] != 0d))
    throw new ArgumentException("A real stop boundary must have an exact zero speed limit.");

HasStopBoundary = hasStopBoundary;
```

公开：

```csharp
public bool HasStopBoundary { get; }
public double PathUpperBoundS => PathS[PathS.Count - 1];
[Obsolete("Use PathUpperBoundS.")]
public double TerminalPathS => PathUpperBoundS;
```

- [ ] **Step 4: 按模式构建停止速度上限**

`RollingContinuation` 的 `stopping` 数组全部填 `directionMaximum`，最终组合速度不再因局部窗口结束而变成零。

`ApproachStopBoundary` 和 `ExactStopAtBoundary` 对每个 sample 使用：

```csharp
double remaining = input.StopBoundaryPathS - samplePathS;
double stoppingValue = JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(
    Math.Max(0d, remaining), maximumAcceleration,
    maximumDeceleration, maximumJerk, directionMaximum);
```

真实边界采样严格赋值零。删除旧的 `sqrt(2d * maximumDeceleration * remaining)` 和旧离散减速尾部公式；采样加密站根据新停车模型的距离反求点生成。

- [ ] **Step 5: 运行测试并提交**

```powershell
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model
git add -- ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimit.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs
git commit -m "feat: keep rolling speed envelopes open"
```

Expected: `PASS longitudinal-model`。

---

### Task 4: 条件化 ST QP 终端约束与独立验证

**Files:**
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs`
- Test: `ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs`

**Interfaces:**
- Consumes: `LongitudinalPlanningInput.Mode`、`PathUpperBoundS`、`StopBoundaryPathS`。
- Produces: 滚动/接近无终端等式、准确停车对边界锚点和内部稳定尾逐 knot 固定 `S/U/A` 的 QP。
- Consumers: Task 5 optimizer。

- [ ] **Step 1: 扩展约束结构测试**

在现有 `VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints` 中分别构建滚动和准确停车输入并断言：

```csharp
Verification.Equal(0, CountExactEqualityRows(rollingProblem,
    new Dictionary<int, double> { { layout.S(4), 1d } }, rollingInput.PathUpperBoundS),
    "rolling has no exact terminal S");
Verification.Equal(0, CountExactEqualityRows(rollingProblem,
    new Dictionary<int, double> { { layout.U(4), 1d } }, 0d),
    "rolling has no exact terminal U");
Verification.Equal(0, CountExactEqualityRows(rollingProblem,
    new Dictionary<int, double> { { layout.A(4), 1d } }, 0d),
    "rolling has no exact terminal A");

int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
    integrated.KnotTimes, configuration.Scheduling.OutputTimeStepSeconds);
for (int index = stabilizationStart; index < layout.KnotCount; index++)
{
    Verification.Equal(1, CountExactEqualityRows(stopProblem,
        new Dictionary<int, double> { { layout.S(index), 1d } }, stopInput.StopBoundaryPathS),
        "stop tail exact S " + index);
    Verification.Equal(1, CountExactEqualityRows(stopProblem,
        new Dictionary<int, double> { { layout.U(index), 1d } }, 0d),
        "stop tail exact U " + index);
    Verification.Equal(1, CountExactEqualityRows(stopProblem,
        new Dictionary<int, double> { { layout.A(index), 1d } }, 0d),
        "stop tail exact A " + index);
}
```

增加验证器测试：滚动候选末速非零应通过；准确停车候选在稳定尾任一 knot 出现 `S/U/A` 偏差应失败；Approach 候选任一点违反 `S + stoppingDistance(U,A) <= StopBoundaryPathS` 应失败。

- [ ] **Step 2: 运行失败测试**

```powershell
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model
```

Expected: 旧 QP 仍为所有模式添加终端 `S/U`，且没有终端 `A`，测试失败。

- [ ] **Step 3: 动态计算约束行并条件化终端约束**

在 `TryBuild` 中使用：

```csharp
int stabilizationStart = input.Mode == EmLongitudinalMode.ExactStopAtBoundary
    ? LongitudinalTerminalSchedule.GetStabilizationStartIndex(expectedTimes,
        input.Configuration.Scheduling.OutputTimeStepSeconds)
    : layout.KnotCount;
int stationaryKnotCount = layout.KnotCount - stabilizationStart;
int terminalRows = 3 * stationaryKnotCount;
int expectedRows = 8 * layout.KnotCount - 2 + terminalRows;
var constraints = new SparseTripletBuilder(expectedRows, layout.VariableCount);
var lower = new List<double>(expectedRows);
var upper = new List<double>(expectedRows);
```

将 `AddExactStartAndTerminal` 拆成：

```csharp
AddExactStart(input, layout, constraints, lower, upper, ref row);
if (input.Mode == EmLongitudinalMode.ExactStopAtBoundary)
    AddExactStopTail(input, layout, stabilizationStart,
        constraints, lower, upper, ref row);
```

`AddExactStopTail` 对 `stabilizationStart..KnotCount-1` 的每个 knot 依次添加 `S=StopBoundaryPathS`、`U=0`、`A=0` 三个等式。

- [ ] **Step 4: 条件化候选验证和规范化**

始终验证普通范围和动力学。仅准确停车执行：

```csharp
int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
    candidate.KnotTimes, input.Configuration.Scheduling.OutputTimeStepSeconds);
for (int index = stabilizationStart; index < candidate.S.Count; index++)
{
    if (!AreClose(candidate.S[index], input.StopBoundaryPathS, tolerance) ||
        !AreClose(candidate.U[index], 0d, tolerance) ||
        !AreClose(candidate.A[index], 0d, tolerance))
    {
        failureReason = "ST candidate does not satisfy the exact stabilized S/U/A stop tail at knot " + index + ".";
        return false;
    }
}
```

Approach 和 ExactStop 逐点执行：

```csharp
if (!JerkLimitedStoppingMath.TryCalculate(candidate.U[index], candidate.A[index],
        maximumDeceleration, maximumJerk, out JerkLimitedStoppingProfile stop, out _) ||
    candidate.S[index] + stop.DistanceMeters > input.StopBoundaryPathS + tolerance)
{
    failureReason = "ST candidate leaves the jerk-limited stoppable set at knot " + index + ".";
    return false;
}
```

只在准确停车模式将 canonical 的 `stabilizationStart..last` 全部写成 `StopBoundaryPathS/0/0`。

- [ ] **Step 5: 运行测试并提交**

```powershell
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model
git add -- ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs
git commit -m "feat: apply ST stop constraints only at real boundaries"
```

Expected: `PASS longitudinal-model`。

---

### Task 5: 按模式构造纵向初始解和外层迭代

**Files:**
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalObjectiveBuilder.cs`
- Test: `ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs`

**Interfaces:**
- Consumes: Tasks 1–4 的停车模型、速度包络和条件化约束。
- Produces: 可供 OSQP 热启动的滚动、接近和准确停车候选。

- [ ] **Step 1: 写出滚动初始解和 point-21 回归测试**

在 `LongitudinalIntegrationChecks.Run()` 增加：

```csharp
VerifiesRollingOptimizationKeepsANonzeroTerminalSpeed();
VerifiesExactStopIncludesAStabilizationTail();
```

滚动测试使用直线 `LateralPath`、`TimeHorizonSeconds=2d`、`OutputTimeStepSeconds=0.1d`、`PathUpperBoundS=5d`，由现有 fake solver 返回优化器初始/严格 fallback，断言：

```csharp
Verification.True(result.Candidate.S[result.Candidate.S.Count - 1] < input.PathUpperBoundS,
    "two-second ST does not consume the five-metre LS window");
Verification.True(result.Candidate.U[result.Candidate.U.Count - 1] > 0d,
    "rolling ST keeps nonzero terminal speed");
```

准确停车测试计算 `stabilizationStart` 并断言整个内部稳定尾：

```csharp
int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
    result.Candidate.KnotTimes, input.Configuration.Scheduling.OutputTimeStepSeconds);
for (int index = stabilizationStart; index < result.Candidate.S.Count; index++)
{
    Verification.NearlyEqual(input.StopBoundaryPathS, result.Candidate.S[index],
        "stop-tail S " + index);
    Verification.NearlyEqual(0d, result.Candidate.U[index], "stop-tail U " + index);
    Verification.NearlyEqual(0d, result.Candidate.A[index], "stop-tail A " + index);
}
```

- [ ] **Step 2: 运行失败测试**

```powershell
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-integration
```

Expected: 滚动候选仍被强制到路径末端零速，测试失败。

- [ ] **Step 3: 将初始迭代分派到三个构造器**

调用点先改为 `CreateInitialIterate(input, speedLimit)`，方法按模式分派：

```csharp
switch (input.Mode)
{
    case EmLongitudinalMode.RollingContinuation:
        return CreateRollingSeed(input, times, speedLimit);
    case EmLongitudinalMode.ApproachStopBoundary:
        return CreateApproachSeed(input, times, speedLimit);
    case EmLongitudinalMode.ExactStopAtBoundary:
        return CreateExactStopSeed(input, times);
    default:
        throw new ArgumentOutOfRangeException(nameof(input.Mode));
}
```

`CreateRollingSeed` 从 `S0=0,U0,A0` 开始，在每个 interval 内选择使速度向 `speedLimit.MaximumSpeedAt(S[k])` 靠近的 jerk，并使用 `LongitudinalCandidate.Integrate` 相同公式积分；遇到速度、加速度或 PathUpperBoundS 时缩小 jerk，不写死末项。

`CreateApproachSeed` 生成“允许巡航段 + 完整 jerk 受限停车段”，然后仅保留 `TimeHorizonSeconds` 内的前缀；每个 knot 的参考速度不超过 stopping envelope。

`CreateExactStopSeed` 先通过 `LongitudinalTerminalSchedule` 得到 `stabilizationStart`，只使用 `times[0..stabilizationStart]` 安排受限制的加速/巡航和 Task 1 三阶段停车，使 `stabilizationStart` 准确达到 `S_end,U=0,A=0`。随后将其余 knot 填充为同一 `S_end` 上的 `U=0,A=0,J=0` 内部稳定尾。

- [ ] **Step 4: 移除外层迭代中的无条件末点固定**

`TryCreateEnvelopeIterate` 中只在准确停车模式执行：

```csharp
int stabilizationStart = input.Mode == EmLongitudinalMode.ExactStopAtBoundary
    ? LongitudinalTerminalSchedule.GetStabilizationStartIndex(candidate.KnotTimes,
        input.Configuration.Scheduling.OutputTimeStepSeconds)
    : candidate.S.Count;
if (input.Mode == EmLongitudinalMode.ExactStopAtBoundary &&
    index >= stabilizationStart)
    candidateProgress = input.StopBoundaryPathS;
```

所有对 `input.TerminalPathS` 的上界用途改为 `PathUpperBoundS`。终点用途改为 `StopBoundaryPathS`，并且必须位于准确停车分支中。

`LongitudinalObjectiveBuilder` 保留速度、加速度、jerk 和上一轨迹目标；`TerminalAcceleration` 软目标只对非准确停车模式添加，准确停车的锚点和内部稳定尾已经由硬约束保证 `A=0`。

- [ ] **Step 5: 运行纵向模型与集成测试并提交**

```powershell
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-integration
git add -- ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalObjectiveBuilder.cs ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs
git commit -m "feat: seed rolling and exact-stop ST profiles"
```

Expected: `PASS longitudinal-model`、`PASS longitudinal-integration`。

---

### Task 6: 条件化轨迹采样、边界锚点和发布验证

**Files:**
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectoryMetadata.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmTrajectoryValidator.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs`
- Test: `ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs`
- Test: `ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs`
- Test: `ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs`
- Test: `ClumsyPilot/tests/EMPlannerVerificationHost/CoordinatorChecks.cs`

**Interfaces:**
- Consumes: `EmLongitudinalMode` 和经过验证的纵向候选。
- Produces: 滚动轨迹不带保持点；准确停车轨迹带安全保持点和真实边界锚点。

- [ ] **Step 1: 写出滚动采样和终端 jerk 回归测试**

在 `TrajectoryChecks.Run()` 增加：

```csharp
VerifiesRollingTrajectoryHasNoSyntheticStopTail();
VerifiesExactStopHoldHasNoJerkDiscontinuity();
```

滚动测试使用 21 个 `0.1s` knot、非零末速和非零但受限的末加速度，断言点数仍为 21、末点 `BoundaryType.None`、末速非零。

准确停车回归构造的速度有限差分状态：

```text
t=1.8: U=0.0025, A=-0.05
t=1.9: U=0, A=0, exact stop anchor
t=2.0: U=0, A=0, QP stabilization point
t=2.1: U=0, A=0, first external hold
```

然后运行 `EmTrajectoryValidator`，断言通过，并显式计算：

```csharp
double terminalFiniteDifferenceAcceleration =
    (anchor.SignedLongitudinalVelocity - previousMoving.SignedLongitudinalVelocity) /
    (anchor.TimeFromStart - previousMoving.TimeFromStart);
double stabilizationAcceleration =
    (stabilization.SignedLongitudinalVelocity - anchor.SignedLongitudinalVelocity) /
    (stabilization.TimeFromStart - anchor.TimeFromStart);
double externalHoldAcceleration =
    (firstExternalHold.SignedLongitudinalVelocity - stabilization.SignedLongitudinalVelocity) /
    (firstExternalHold.TimeFromStart - stabilization.TimeFromStart);
double jerkIntoStabilization = (stabilizationAcceleration -
    terminalFiniteDifferenceAcceleration) /
    (stabilization.TimeFromStart - anchor.TimeFromStart);
double jerkIntoExternalHold = (externalHoldAcceleration - stabilizationAcceleration) /
    (firstExternalHold.TimeFromStart - stabilization.TimeFromStart);
Verification.True(Math.Abs(jerkIntoStabilization) <= 0.5d,
    "jerk into QP stabilization stays within the limit");
Verification.NearlyEqual(0d, jerkIntoExternalHold,
    "jerk into the external hold is zero");
```

再构造内部稳定尾任一 knot 的 `S/U/A` 非零偏差，断言采样器或发布验证器拒绝；构造真实边界锚点存储加速度非零的轨迹，断言发布验证器以 `TerminalAccelerationNotZero` 拒绝。

- [ ] **Step 2: 运行测试并确认旧采样器拒绝滚动末速**

```powershell
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory
```

Expected: 旧 `TrajectorySampleSchedule` 抛出“requires an exact zero-speed terminal candidate”或点数/边界断言失败。

- [ ] **Step 3: 给元数据增加纵向模式**

`EmTrajectoryMetadata` 构造函数末尾增加 `EmLongitudinalMode longitudinalMode`，验证枚举并公开：

```csharp
public EmLongitudinalMode LongitudinalMode { get; }
```

`EmPlanningService` 构造元数据时传 `horizon.LongitudinalMode`。`TrajectoryChecks` 中 Goal/GearSwitch 元数据传 `ExactStopAtBoundary`；`TrajectoryObservationChecks`、`ExecutorChecks` 和 `CoordinatorChecks` 中代表运动中轨迹的 fixture 传 `RollingContinuation`，代表完成停车的 fixture 传 `ExactStopAtBoundary`。

- [ ] **Step 4: 按模式构建采样计划**

`TrajectorySampleSchedule` 构造函数增加 mode：

```csharp
if (mode != EmLongitudinalMode.ExactStopAtBoundary)
{
    Samples = new ReadOnlyCollection<TrajectorySample>(candidateSamples);
    TerminalAnchorSampleIndex = -1;
    return;
}
int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
    candidate.KnotTimes, outputTimeStepSeconds);
double stopPathS = candidate.S[stabilizationStart];
for (int index = stabilizationStart; index < candidate.S.Count; index++)
{
    if (Math.Abs(candidate.S[index] - stopPathS) > ZeroTolerance ||
        Math.Abs(candidate.U[index]) > ZeroTolerance ||
        Math.Abs(candidate.A[index]) > ZeroTolerance ||
        (index < candidate.J.Count && Math.Abs(candidate.J[index]) > ZeroTolerance))
        throw new ArgumentException("An exact stop requires a stationary S/U/A/J tail.");
}
TerminalAnchorSampleIndex = stabilizationStart;
AppendHoldSamples(candidateSamples, outputTimeStepSeconds, holdDurationSeconds);
```

公开只读 `TerminalAnchorSampleIndex`。保持段从优化末点之后开始，不复制同时间戳点。

- [ ] **Step 5: 只为准确停车标记边界锚点**

`EmTrajectoryAssembler` 使用：

```csharp
bool isTerminalAnchor = metadata.LongitudinalMode ==
    EmLongitudinalMode.ExactStopAtBoundary &&
    index == schedule.TerminalAnchorSampleIndex;
EmBoundaryType boundaryType = isTerminalAnchor
    ? ToBoundaryType(metadata.TerminalType)
    : EmBoundaryType.None;
```

滚动时间末点和 Approach 时间末点都保持 `BoundaryType.None`。

- [ ] **Step 6: 条件化世界坐标发布验证**

在 `EmTrajectoryValidationFailure` 增加：

```csharp
TerminalAccelerationNotZero,
```

始终执行普通运动学、碰撞和有限差分 jerk 检查。仅准确停车模式寻找真实边界锚点，检查锚点存储的 `LongitudinalAcceleration`、速度和 yaw rate 近零，并验证从锚点到轨迹末尾保持同一 PathS、零速和零状态加速度。滚动/Approach 不调用 `FindTerminalAnchor`，也不检查零末态。

- [ ] **Step 7: 运行轨迹测试并提交**

```powershell
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- coordinator
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- executor
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory-observation
git add -- ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectoryMetadata.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmTrajectoryValidator.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs ClumsyPilot/tests/EMPlannerVerificationHost/CoordinatorChecks.cs
git commit -m "feat: publish rolling trajectories without stop tails"
```

Expected: `PASS trajectory`、`PASS coordinator`、`PASS executor`、`PASS trajectory-observation`，point-21 回归通过。

---

### Task 7: 上一轨迹纵向软参考

**Files:**
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPreviousTrajectorySeedBuilder.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs`
- Test: `ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs`
- Test: `ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs`

**Interfaces:**
- Produces: `LongitudinalPreviousTrajectorySeed`，包含相同数量的 PathS 和进度速度样本。
- Consumes: 上一条 `EmTrajectory`、新 `LateralPath`、新轨迹生效时间和新 knot 时间。

- [ ] **Step 1: 写出时间对齐、方向过滤和 PathS 映射测试**

添加测试：当前 LateralPath 是 `X/PathS=0..1m` 的直线；上一轨迹 `EffectiveAtUtc=10s`，在 `t=0,0.1,0.2,0.3,0.4s` 分别位于 `X=0,0.1,0.2,0.3,0.4m`；以 `newEffectiveAtUtc=10.2s`、新 knot `{0,0.1,0.2}` 重采样。断言：

```csharp
Verification.Equal(3, seed.PathS.Count, "previous seed path-S count");
Verification.Equal(3, seed.ProgressSpeedMetersPerSecond.Count,
    "previous seed speed count");
Verification.NearlyEqual(0.20d, seed.PathS[0],
    "previous seed begins at new absolute effective time");
Verification.True(seed.PathS[1] >= seed.PathS[0] && seed.PathS[2] >= seed.PathS[1],
    "previous seed progress is monotone");
```

再用不同方向、不同 segment 和已超出上一轨迹时间范围的输入，断言返回空 seed。

- [ ] **Step 2: 运行失败测试**

```powershell
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model
```

Expected: 编译失败，seed builder 不存在。

- [ ] **Step 3: 实现上一轨迹重采样器**

公开只读结果：

```csharp
public sealed class LongitudinalPreviousTrajectorySeed
{
    public IReadOnlyList<double> PathS { get; }
    public IReadOnlyList<double> ProgressSpeedMetersPerSecond { get; }
    public static LongitudinalPreviousTrajectorySeed Empty { get; }
}
```

builder 的入口：

```csharp
public LongitudinalPreviousTrajectorySeed Build(EmTrajectory previous,
    LateralPath currentPath, DateTimeOffset newEffectiveAtUtc,
    IReadOnlyList<double> newKnotTimes, int segmentIndex,
    TravelDirection direction)
```

逐 knot 使用绝对时间：

```text
sampleUtc = newEffectiveAtUtc + newKnotTime
previousTime = sampleUtc - previous.Metadata.EffectiveAtUtc
```

在线性插值上一轨迹世界坐标和 signed speed 后，把世界点投影到 `currentPath.Points[i] -> Points[i+1]` 的折线段，取距离最小且不小于上一投影 PathS 的结果。进度速度使用 `abs(interpolatedSignedSpeed)`。任一输入不兼容时返回 `Empty`，不得抛出影响规划发布的异常。

- [ ] **Step 4: 在服务中传递模式和纵向 seed**

`EmPlanningService` 在 lateral path 成功后创建 knot 时间并调用 builder：

```csharp
IReadOnlyList<double> knotTimes = LongitudinalCandidate.CreateKnotTimes(
    configuration.Scheduling.TimeHorizonSeconds,
    configuration.Scheduling.OutputTimeStepSeconds);
LongitudinalPreviousTrajectorySeed previous =
    new LongitudinalPreviousTrajectorySeedBuilder().Build(
        request.PreviousTrajectory, lateral.Path, request.EffectiveAtUtc,
        knotTimes, segment.SegmentIndex, segment.Direction);

var longitudinalInput = new LongitudinalPlanningInput(
    lateral.Path, segment.Direction, initialProgressSpeed,
    initialAcceleration, horizon.TerminalType,
    horizon.LongitudinalMode, configuration,
    previous.PathS, previous.ProgressSpeedMetersPerSecond);
```

元数据同时写入 `horizon.LongitudinalMode`。服务切片使用 `horizon.WindowEndReferenceS`。

- [ ] **Step 5: 验证服务不再传空 seed**

在 `EmPlanningServiceChecks` 的 scripted solver/capture helper 中记录纵向 QP目标项。带有效上一轨迹时断言 previous-S 和 previous-U 权重产生非零对应 Hessian/linear cost；无效上一轨迹时仍然成功且无该项。

- [ ] **Step 6: 运行并提交**

```powershell
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-planning-service
git add -- ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPreviousTrajectorySeedBuilder.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs
git commit -m "feat: reuse prior trajectory in longitudinal planning"
```

Expected: `PASS longitudinal-model`、`PASS em-planning-service`。

---

### Task 8: 服务模式流转、Goal/GearSwitch 与多周期回归

**Files:**
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs`
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs`
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/EmFixtureFactory.cs`

**Interfaces:**
- Consumes: Tasks 1–7 的完整规划链路。
- Produces: 证明 `RollingContinuation -> ApproachStopBoundary -> ExactStopAtBoundary` 的服务级行为。

- [ ] **Step 1: 写多周期模式流转测试**

创建一条足够长的无障碍直线路径和三组车辆状态：边界在空间窗口外、边界在窗口内但不在时间停车可达范围内、边界在准确停车可达范围内。每次调用 `EmPlanningService.Plan`，断言：

```csharp
Verification.Equal(EmLongitudinalMode.RollingContinuation,
    rolling.Trajectory.Metadata.LongitudinalMode, "cycle 1 rolls");
Verification.True(rolling.Trajectory.Points[rolling.Trajectory.Points.Count - 1]
    .SignedLongitudinalVelocity != 0d, "cycle 1 has nonzero terminal speed");

Verification.Equal(EmLongitudinalMode.ApproachStopBoundary,
    approach.Trajectory.Metadata.LongitudinalMode, "cycle 2 approaches");

Verification.Equal(EmLongitudinalMode.ExactStopAtBoundary,
    exact.Trajectory.Metadata.LongitudinalMode, "cycle 3 stops");
Verification.NearlyEqual(0d, exact.Trajectory.Points[exactAnchor]
    .SignedLongitudinalVelocity, "goal speed is zero");
Verification.True(exact.Trajectory.Points.Count > exactAnchor + 1,
    "goal anchor is followed by a QP stabilization point");
Verification.NearlyEqual(exact.Trajectory.Points[exactAnchor].PathS,
    exact.Trajectory.Points[exactAnchor + 1].PathS,
    "goal stabilization keeps the stop position");
Verification.NearlyEqual(0d, exact.Trajectory.Points[exactAnchor + 1]
    .SignedLongitudinalVelocity, "goal stabilization speed is zero");
```

为 GearSwitch 使用 `CreateGearPairReferencePath()` 重复准确停车检查，并确认发布轨迹不包含下一方向段点。

- [ ] **Step 2: 写五米空间窗口/两秒时间范围回归**

配置：

```csharp
configuration.Scheduling.DistanceHorizonMeters = 5d;
configuration.Scheduling.TimeHorizonSeconds = 2d;
configuration.Scheduling.OutputTimeStepSeconds = 0.1d;
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 0.2d;
```

断言发布点数为 21，最后一个 PathS 小于 5m，末速非零，并且发布验证通过。

- [ ] **Step 3: 运行失败测试并完成最小服务修正**

```powershell
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-core-all
```

Expected before fixture updates: service fixture 或 fake solver 仍假定每条轨迹零速停车，至少一个新断言失败。若失败来自 Tasks 1–7 的生产实现，返回所属任务修正并重新执行其测试；本任务只修改测试 fixture，不放宽生产校验。

- [ ] **Step 4: 更新旧 rolling service fixture**

旧测试通过 `DistanceHorizonMeters=0.003m` 人工制造 rolling terminal；改为使用大于配置停车前视下限的 DistanceHorizon 和更长的参考路径。scripted solver 按 `EmLongitudinalMode` 返回：

- rolling/approach：非零末速、无保持尾；
- exact stop：真实边界锚点及其后的 QP 内部稳定尾 `S/U/A` 准确，允许继续追加外部保持尾。

- [ ] **Step 5: 运行并提交**

```powershell
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-core-all
git add -- ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs ClumsyPilot/tests/EMPlannerVerificationHost/EmFixtureFactory.cs
git commit -m "test: cover rolling-to-stop EM planning flow"
```

Expected: `PASS longitudinal-model`、`PASS longitudinal-integration`、`PASS trajectory`、`PASS em-planning-service`。

---

### Task 9: 观察日志和文档

**Files:**
- Modify: `ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs`
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs`
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md`
- Modify: `ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md`

**Interfaces:**
- Consumes: 轨迹元数据中的 `LongitudinalMode` 和 Task 1 停车能力计算。
- Produces: 启动配置停车能力说明，以及每条成功轨迹的模式和末态摘要。

- [ ] **Step 1: 写日志失败测试**

在配置诊断断言中增加：

```text
maximumJerkLimitedStopDistance=
maximumJerkLimitedStopDuration=
requiredDistanceHorizon=
```

在成功轨迹摘要断言中增加：

```text
longitudinalMode=RollingContinuation
terminalSpeed=
terminalAcceleration=
```

服务失败原因断言至少覆盖：

```text
longitudinalMode=
remainingToBoundary=
minimumStoppingDistance=
minimumStoppingDuration=
maximumStoppedReachableDistance=
```

- [ ] **Step 2: 运行并确认缺少字段**

```powershell
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory-observation
```

Expected: 新字段断言失败。

- [ ] **Step 3: 扩展一次性配置和每周期摘要**

`CreateConfiguration` 使用正向/反向最大速度的较坏情况调用停车模型，并打印：

```text
maximumJerkLimitedStopDistance=0.259m
maximumJerkLimitedStopDuration=1.800s
requiredDistanceHorizon=0.299m
```

`CreateTrajectorySummary` 增加：

```text
longitudinalMode=RollingContinuation
terminalSpeed=0.180m/s
terminalAcceleration=0.000m/s2
```

这些配置仍只在 session 启动时打印一次，不加入 UI 每帧重绘块。

- [ ] **Step 4: 更新 README**

在 EMPlanner README 明确：

```text
DistanceHorizonMeters controls the LS reference window.
TimeHorizonSeconds controls the ST output duration.
Only Goal/GearSwitchApproach may require an exact zero-speed terminal.
RollingContinuation and ApproachStopBoundary may publish nonzero terminal speed.
```

在 MovementTest README 明确仍为 `OBSERVE_ONLY`，滚动轨迹不会下发底盘。

- [ ] **Step 5: 运行并提交**

```powershell
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory-observation
git add -- ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md
git commit -m "docs: explain rolling longitudinal planning diagnostics"
```

Expected: `PASS trajectory-observation`。

---

### Task 10: 全量回归和目标机 OSQP 验证

**Files:**
- No file changes planned; this task is verification-only。
- Do not modify: unrelated dirty PathSmoothing、CoarsePath、Map or user configuration files。

**Interfaces:**
- Consumes: all prior tasks。
- Produces: verified implementation ready for observation-only target-machine testing。

- [ ] **Step 1: 运行核心回归**

```powershell
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-core-all
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- coordinator
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- executor
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory-observation
```

Expected: 每条命令分别输出它在 `Program.cs` 中定义的 `PASS` 行；不出现新的编译错误或验证失败。

- [ ] **Step 2: 构建主工程**

```powershell
dotnet build ClumsyPilot/ClumsyPilot.csproj -p:ExcludeLegacyAutoAvoidance=true
```

Expected: Build succeeded。已有 obsolete warning 可以保留，但不得新增本计划相关 warning。

- [ ] **Step 3: 在装有正确 OSQP 原生依赖的目标机运行真实求解探针**

```powershell
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-real-osqp-probe
```

Expected: `PASS longitudinal-real-osqp`。若原生 DLL 无法加载，只记录为目标机依赖问题，不通过修改纵向逻辑或放宽残差阈值绕过。

- [ ] **Step 4: 运行 MovementTest observation-only 实测**

使用当前手工参数启动 `TrajectoryObservationMovementTest`，确认：

1. 启动时只打印一次规划配置和最大 jerk 受限停车能力；
2. 远离 Goal 时发布 `RollingContinuation`，末速非零且无 hold；
3. 接近 Goal 时出现 `ApproachStopBoundary`；
4. 最终出现 `ExactStopAtBoundary`，终端 `U=0,A=0`；
5. 不再出现 point 21 `JerkLimitExceeded`；
6. 终端持续打印 `OBSERVE_ONLY: no chassis command is sent.`。

- [ ] **Step 5: 检查只包含计划内改动**

```powershell
git status --short
git log --oneline -12
```

逐个核对 Tasks 1–9 的提交；不得暂存或提交用户现有 PathSmoothing、CoarsePath、Map 和 MovementTest 手工配置改动。

- [ ] **Step 6: 处理最终回归结果**

若 Step 1–4 失败，返回引入该行为的 Task 1–9，在该任务列出的文件范围和测试命令内修正并追加对应任务提交；不得用一个无法确定所有权的“最终修复”提交混合多个模块。全部通过时不创建空提交。