diff --git a/docs/superpowers/plans/2026-08-05-em-longitudinal-rolling-planning.md b/docs/superpowers/plans/2026-08-05-em-longitudinal-rolling-planning.md new file mode 100644 index 0000000..5a489ec --- /dev/null +++ b/docs/superpowers/plans/2026-08-05-em-longitudinal-rolling-planning.md @@ -0,0 +1,1435 @@ +# 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)`、扩展后的 `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 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 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 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(), Array.Empty()); + 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 { { layout.S(4), 1d } }, rollingInput.PathUpperBoundS), + "rolling has no exact terminal S"); +Verification.Equal(0, CountExactEqualityRows(rollingProblem, + new Dictionary { { layout.U(4), 1d } }, 0d), + "rolling has no exact terminal U"); +Verification.Equal(0, CountExactEqualityRows(rollingProblem, + new Dictionary { { 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 { { layout.S(index), 1d } }, stopInput.StopBoundaryPathS), + "stop tail exact S " + index); + Verification.Equal(1, CountExactEqualityRows(stopProblem, + new Dictionary { { layout.U(index), 1d } }, 0d), + "stop tail exact U " + index); + Verification.Equal(1, CountExactEqualityRows(stopProblem, + new Dictionary { { 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(expectedRows); +var upper = new List(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(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 PathS { get; } + public IReadOnlyList ProgressSpeedMetersPerSecond { get; } + public static LongitudinalPreviousTrajectorySeed Empty { get; } +} +``` + +builder 的入口: + +```csharp +public LongitudinalPreviousTrajectorySeed Build(EmTrajectory previous, + LateralPath currentPath, DateTimeOffset newEffectiveAtUtc, + IReadOnlyList 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 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,在该任务列出的文件范围和测试命令内修正并追加对应任务提交;不得用一个无法确定所有权的“最终修复”提交混合多个模块。全部通过时不创建空提交。 diff --git a/docs/superpowers/specs/2026-08-05-em-longitudinal-rolling-planning-design.md b/docs/superpowers/specs/2026-08-05-em-longitudinal-rolling-planning-design.md index f3198a5..cb5478b 100644 --- a/docs/superpowers/specs/2026-08-05-em-longitudinal-rolling-planning-design.md +++ b/docs/superpowers/specs/2026-08-05-em-longitudinal-rolling-planning-design.md @@ -310,8 +310,8 @@ terminal S/U/A when applicable 停车距离不足时返回: ```text -StoppingDistanceInsufficient: remaining=..., required=..., shortage=..., -speed=..., acceleration=..., decelerationLimit=..., jerkLimit=... +StoppingDistanceInsufficient: remaining=0.080, required=0.127, shortage=0.047, +speed=0.200, acceleration=0.000, decelerationLimit=0.300, jerkLimit=0.500 ``` 发布成功摘要继续报告点数、持续时间、实际 PathS 长度和最大速度/加速度/jerk。滚动轨迹的实际 PathS 长度不得被错误报告为 `DistanceHorizonMeters`。