First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
Files
34f99526e322dcbaa568243355b3ebf0ca9075a7
ParkingRobot/docs/superpowers/plans/2026-08-05-em-longitudinal-rolling-planning.md
T

65 KiB
Raw Blame History

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() 开头加入：

VerifiesJerkLimitedStoppingProfileEndsAtRest();
VerifiesStoppedReachabilityUsesTheSameJerkModel();

新增完整测试：

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:

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

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

  • Step 3: 新建停车数学并迁移旧类型

在新文件定义：

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:

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

Expected: PASS longitudinal-model。

  • Step 5: 独立提交停车数学
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() 的断言并拆成三个场景：

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:

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

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

  • Step 3: 增加模式和选择结果

创建：

namespace MultiWheelC.TrajectoryPlanning.EMPlanner;

public enum EmLongitudinalMode
{
    RollingContinuation,
    ApproachStopBoundary,
    ExactStopAtBoundary,
}

将 PlanningHorizonSelection 定义调整为：

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。

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

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: 重写窗口选择器

核心分支必须等价于：

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，并增加：

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;

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

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 中计算正向和反向最大速度下的保守停车距离，取较大值并增加：

previewReserve = maxSpeed * ReplanPeriodSeconds
requiredDistanceHorizon = worstStoppingDistance + previewReserve

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

  • Step 7: 更新所有 LongitudinalPlanningInput 构造调用并运行测试

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

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: 独立提交模式与窗口语义
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: 写出滚动末端非零和真实边界零速测试

加入：

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: 运行测试并确认滚动末端仍被置零
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model

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

  • Step 3: 修改 PathSpeedLimit 契约

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

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;

公开：

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 使用：

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

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

  • Step 5: 运行测试并提交
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 中分别构建滚动和准确停车输入并断言：

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: 运行失败测试
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model

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

  • Step 3: 动态计算约束行并条件化终端约束

在 TryBuild 中使用：

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 拆成：

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: 条件化候选验证和规范化

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

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 逐点执行：

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: 运行测试并提交
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() 增加：

VerifiesRollingOptimizationKeepsANonzeroTerminalSpeed();
VerifiesExactStopIncludesAStabilizationTail();

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

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 并断言整个内部稳定尾：

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: 运行失败测试
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-integration

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

  • Step 3: 将初始迭代分派到三个构造器

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

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 中只在准确停车模式执行：

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: 运行纵向模型与集成测试并提交
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() 增加：

VerifiesRollingTrajectoryHasNoSyntheticStopTail();
VerifiesExactStopHoldHasNoJerkDiscontinuity();

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

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

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，断言通过，并显式计算：

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: 运行测试并确认旧采样器拒绝滚动末速
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory

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

  • Step 3: 给元数据增加纵向模式

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

public EmLongitudinalMode LongitudinalMode { get; }

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

  • Step 4: 按模式构建采样计划

TrajectorySampleSchedule 构造函数增加 mode：

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 使用：

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

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

  • Step 6: 条件化世界坐标发布验证

在 EmTrajectoryValidationFailure 增加：

TerminalAccelerationNotZero,

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

  • Step 7: 运行轨迹测试并提交
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} 重采样。断言：

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: 运行失败测试
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model

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

  • Step 3: 实现上一轨迹重采样器

公开只读结果：

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

builder 的入口：

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

逐 knot 使用绝对时间：

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：

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: 运行并提交
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，断言：

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: 写五米空间窗口/两秒时间范围回归

配置：

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

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

  • Step 3: 运行失败测试并完成最小服务修正
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: 运行并提交

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: 写日志失败测试

在配置诊断断言中增加：

maximumJerkLimitedStopDistance=
maximumJerkLimitedStopDuration=
requiredDistanceHorizon=

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

longitudinalMode=RollingContinuation
terminalSpeed=
terminalAcceleration=

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

longitudinalMode=
remainingToBoundary=
minimumStoppingDistance=
minimumStoppingDuration=
maximumStoppedReachableDistance=
  • Step 2: 运行并确认缺少字段
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory-observation

Expected: 新字段断言失败。

  • Step 3: 扩展一次性配置和每周期摘要

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

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

CreateTrajectorySummary 增加：

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

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

  • Step 4: 更新 README

在 EMPlanner README 明确：

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: 运行并提交
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: 运行核心回归

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: 构建主工程
dotnet build ClumsyPilot/ClumsyPilot.csproj -p:ExcludeLegacyAutoAvoidance=true

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

  • Step 3: 在装有正确 OSQP 原生依赖的目标机运行真实求解探针
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: 检查只包含计划内改动
git status --short
git log --oneline -12

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

  • Step 6: 处理最终回归结果

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

Reference in New Issue View Git Blame Copy Permalink