1436 lines
65 KiB
Markdown
1436 lines
65 KiB
Markdown
# EM Longitudinal Rolling Planning Implementation Plan
|
||
|
||
> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking.
|
||
|
||
**Goal:** 修复 EM 纵向 ST 规划，使局部空间窗口能够连续滚动，只在真实 Goal 或 GearSwitchApproach 边界以 jerk 受限的 `S=S_end、U=0、A=0` 状态停车。
|
||
|
||
**Architecture:** 将 LS 空间窗口、ST 时间范围和真实停车边界拆成独立概念，由 `EmLongitudinalMode` 统一驱动速度包络、QP终端约束、初始迭代、轨迹采样和发布验证。使用与 QP 恒 jerk 动力学一致的完整 S 曲线停车模型，并将上一条已发布轨迹按绝对时间重采样为纵向软参考。
|
||
|
||
**Tech Stack:** C# 10、.NET Standard 2.0、OSQP QP接口、`EMPlannerVerificationHost`（.NET 10 Windows 控制台验证宿主）、PowerShell、Git。
|
||
|
||
## Global Constraints
|
||
|
||
- 设计规格以 `docs/superpowers/specs/2026-08-05-em-longitudinal-rolling-planning-design.md` 为唯一行为依据。
|
||
- `DistanceHorizonMeters` 是 LS 空间前视范围，不是 ST 必须在本周期走完的距离。
|
||
- `TimeHorizonSeconds` 是 ST 优化时间范围，不包含 `ZeroSpeedHoldSeconds`。
|
||
- 只有真实 `Goal` 或 `GearSwitchApproach` 可以触发准确停车；局部窗口截断点不能触发零速保持。
|
||
- 准确停车必须在 ST 时间范围结束前到达 `S=S_end、U=0、A=0`，并在 QP 内保留至少一个完整 `OutputTimeStepSeconds` 的静止稳定尾；不得放宽速度、加速度、jerk、QP残差或发布验证阈值。
|
||
- 滚动和接近模式允许非零末点速度、加速度及 yaw rate，但仍必须满足所有普通运动学和碰撞限制。
|
||
- `OBSERVE_ONLY` 保持不变；不增加底盘命令、不增加轨迹执行器超时看门狗。
|
||
- 保留工作树中与本计划无关的用户修改；每次只暂存当前任务列出的文件。
|
||
|
||
---
|
||
|
||
## File Structure
|
||
|
||
### 新建文件
|
||
|
||
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmLongitudinalMode.cs`：定义滚动、接近停车边界、准确停车三种纵向行为。
|
||
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/JerkLimitedStoppingMath.cs`：完整 jerk 受限停车曲线、停止距离反求和时间范围内最大可停车行驶距离。
|
||
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalTerminalSchedule.cs`：统一计算准确停车锚点和 QP 内部稳定尾 knot 范围。
|
||
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPreviousTrajectorySeedBuilder.cs`：将上一条世界坐标轨迹按新时间 knot 重采样并映射到新 LateralPath 的 PathS。
|
||
|
||
### 主要修改文件
|
||
|
||
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/PlanningHorizonSelector.cs`：空间窗口和停车可达性选择。
|
||
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanningInput.cs`：拆分路径上界、真实停车边界和纵向模式。
|
||
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimit.cs`：允许滚动路径末端非零速度上限。
|
||
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs`：按模式构建 jerk 受限停车速度包络。
|
||
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs`：仅准确停车添加 `S/U/A` 终端硬约束。
|
||
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs`：按模式验证终端状态及逐点可停车域。
|
||
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs`：按模式生成初始迭代并取消滚动末点强制固定。
|
||
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectoryMetadata.cs`：在发布轨迹中记录纵向模式。
|
||
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs`：只为准确停车追加零速保持点。
|
||
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs`：只为准确停车标记真实边界锚点。
|
||
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmTrajectoryValidator.cs`：条件化终端验证并新增终点加速度校验。
|
||
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmPlanningRequestValidator.cs`：验证空间前视的停车余量。
|
||
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs`：传递纵向模式、上一轨迹种子和模式诊断。
|
||
- `ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs`：显示模式、停车能力和成功末态。
|
||
- `ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md`：记录新的窗口和终端语义。
|
||
|
||
### 测试文件
|
||
|
||
- `ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs`
|
||
- `ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs`
|
||
- `ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs`
|
||
- `ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs`
|
||
- `ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs`
|
||
|
||
---
|
||
|
||
### Task 1: 完整 jerk 受限停车数学
|
||
|
||
**Files:**
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/JerkLimitedStoppingMath.cs`
|
||
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs`
|
||
- Test: `ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs`
|
||
|
||
**Interfaces:**
|
||
- Produces: `JerkLimitedStoppingMath.TryCalculate(double, double, double, double, out JerkLimitedStoppingProfile, out string)`、`MaximumInitialSpeedForDistance(double, double, double, double, double)`、`CalculateMaximumStoppedDistance(double, double, double, double, double, double, double)`。
|
||
- Produces: `JerkLimitedStoppingProfile`，包含距离、持续时间、末速和末加速度。
|
||
- Consumers: Tasks 2、3、4、8。
|
||
|
||
- [ ] **Step 1: 写出停车末态和可达距离失败测试**
|
||
|
||
在 `LongitudinalModelChecks.Run()` 开头加入：
|
||
|
||
```csharp
|
||
VerifiesJerkLimitedStoppingProfileEndsAtRest();
|
||
VerifiesStoppedReachabilityUsesTheSameJerkModel();
|
||
```
|
||
|
||
新增完整测试：
|
||
|
||
```csharp
|
||
private static void VerifiesJerkLimitedStoppingProfileEndsAtRest()
|
||
{
|
||
Verification.True(JerkLimitedStoppingMath.TryCalculate(
|
||
0.20d, 0d, 0.30d, 0.50d,
|
||
out JerkLimitedStoppingProfile profile, out string failure),
|
||
"jerk-limited stop builds: " + failure);
|
||
Verification.True(profile.DistanceMeters > 0d, "stop distance is positive");
|
||
Verification.True(profile.DurationSeconds > 0d, "stop duration is positive");
|
||
Verification.NearlyEqual(0d, profile.FinalSpeedMetersPerSecond,
|
||
"stop ends at zero speed");
|
||
Verification.NearlyEqual(0d, profile.FinalAccelerationMetersPerSecondSquared,
|
||
"stop releases acceleration to zero");
|
||
|
||
Verification.True(JerkLimitedStoppingMath.TryCalculate(
|
||
0.20d, 0.20d, 0.30d, 0.50d,
|
||
out JerkLimitedStoppingProfile accelerating, out failure),
|
||
"positive-acceleration stop builds: " + failure);
|
||
Verification.True(accelerating.DistanceMeters > profile.DistanceMeters,
|
||
"positive initial acceleration needs more stopping distance");
|
||
Verification.NearlyEqual(0d, accelerating.FinalAccelerationMetersPerSecondSquared,
|
||
"positive-acceleration stop also releases acceleration");
|
||
}
|
||
|
||
private static void VerifiesStoppedReachabilityUsesTheSameJerkModel()
|
||
{
|
||
double maximumDistance = JerkLimitedStoppingMath.CalculateMaximumStoppedDistance(
|
||
0d, 0d, 0.20d, 0.20d, 0.30d, 0.50d, 2d);
|
||
Verification.True(maximumDistance > 0d && maximumDistance < 0.40d,
|
||
"two-second stopped reach is finite and below unconstrained cruise distance");
|
||
|
||
double cap = JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(
|
||
maximumDistance, 0.20d, 0.30d, 0.50d, 0.20d);
|
||
Verification.True(cap >= 0d && cap <= 0.20d,
|
||
"distance inversion stays inside the direction speed range");
|
||
}
|
||
```
|
||
|
||
- [ ] **Step 2: 运行测试并确认因类型不存在而失败**
|
||
|
||
Run:
|
||
|
||
```powershell
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model
|
||
```
|
||
|
||
Expected: 编译失败，指出 `JerkLimitedStoppingMath` 和 `JerkLimitedStoppingProfile` 不存在。
|
||
|
||
- [ ] **Step 3: 新建停车数学并迁移旧类型**
|
||
|
||
在新文件定义：
|
||
|
||
```csharp
|
||
public sealed class JerkLimitedStoppingProfile
|
||
{
|
||
internal JerkLimitedStoppingProfile(double distanceMeters, double durationSeconds,
|
||
double finalSpeedMetersPerSecond, double finalAccelerationMetersPerSecondSquared)
|
||
{
|
||
DistanceMeters = distanceMeters;
|
||
DurationSeconds = durationSeconds;
|
||
FinalSpeedMetersPerSecond = finalSpeedMetersPerSecond;
|
||
FinalAccelerationMetersPerSecondSquared = finalAccelerationMetersPerSecondSquared;
|
||
}
|
||
|
||
public double DistanceMeters { get; }
|
||
public double DurationSeconds { get; }
|
||
public double FinalSpeedMetersPerSecond { get; }
|
||
public double FinalAccelerationMetersPerSecondSquared { get; }
|
||
}
|
||
|
||
public static class JerkLimitedStoppingMath
|
||
{
|
||
private const double NumericTolerance = 1e-12d;
|
||
|
||
public static bool TryCalculate(double speed, double acceleration,
|
||
double maximumDeceleration, double maximumJerk,
|
||
out JerkLimitedStoppingProfile profile, out string failureReason)
|
||
{
|
||
profile = null;
|
||
failureReason = string.Empty;
|
||
if (!IsFinite(speed) || speed < 0d || !IsFinite(acceleration) ||
|
||
!IsPositiveFinite(maximumDeceleration) || !IsPositiveFinite(maximumJerk) ||
|
||
acceleration < -maximumDeceleration - NumericTolerance)
|
||
{
|
||
failureReason = "Stopping inputs are outside finite longitudinal bounds.";
|
||
return false;
|
||
}
|
||
if (speed <= NumericTolerance && Math.Abs(acceleration) <= NumericTolerance)
|
||
{
|
||
profile = new JerkLimitedStoppingProfile(0d, 0d, 0d, 0d);
|
||
return true;
|
||
}
|
||
|
||
double unavoidableReleaseLoss = acceleration < 0d
|
||
? acceleration * acceleration / (2d * maximumJerk)
|
||
: 0d;
|
||
if (speed + NumericTolerance < unavoidableReleaseLoss)
|
||
{
|
||
failureReason = "The current negative acceleration cannot be released before speed crosses zero.";
|
||
return false;
|
||
}
|
||
|
||
double peakDeceleration = Math.Sqrt(maximumJerk * speed +
|
||
0.5d * acceleration * acceleration);
|
||
double downDuration;
|
||
double plateauDuration;
|
||
double upDuration;
|
||
if (peakDeceleration <= maximumDeceleration + NumericTolerance)
|
||
{
|
||
peakDeceleration = Math.Min(peakDeceleration, maximumDeceleration);
|
||
downDuration = (acceleration + peakDeceleration) / maximumJerk;
|
||
plateauDuration = 0d;
|
||
upDuration = peakDeceleration / maximumJerk;
|
||
}
|
||
else
|
||
{
|
||
peakDeceleration = maximumDeceleration;
|
||
downDuration = (acceleration + peakDeceleration) / maximumJerk;
|
||
upDuration = peakDeceleration / maximumJerk;
|
||
double speedAfterDown = speed + acceleration * downDuration -
|
||
0.5d * maximumJerk * downDuration * downDuration;
|
||
double releaseLoss = peakDeceleration * peakDeceleration /
|
||
(2d * maximumJerk);
|
||
plateauDuration = (speedAfterDown - releaseLoss) / peakDeceleration;
|
||
}
|
||
if (downDuration < -NumericTolerance || plateauDuration < -NumericTolerance)
|
||
{
|
||
failureReason = "No monotone three-phase jerk-limited stop exists for the current state.";
|
||
return false;
|
||
}
|
||
|
||
downDuration = Math.Max(0d, downDuration);
|
||
plateauDuration = Math.Max(0d, plateauDuration);
|
||
double s = 0d;
|
||
double u = speed;
|
||
double a = acceleration;
|
||
Integrate(ref s, ref u, ref a, -maximumJerk, downDuration);
|
||
Integrate(ref s, ref u, ref a, 0d, plateauDuration);
|
||
Integrate(ref s, ref u, ref a, maximumJerk, upDuration);
|
||
if (Math.Abs(u) > 1e-9d || Math.Abs(a) > 1e-9d || s < -NumericTolerance)
|
||
{
|
||
failureReason = "The jerk-limited stop did not end at zero speed and acceleration.";
|
||
return false;
|
||
}
|
||
profile = new JerkLimitedStoppingProfile(Math.Max(0d, s),
|
||
downDuration + plateauDuration + upDuration, 0d, 0d);
|
||
return true;
|
||
}
|
||
|
||
public static double MaximumInitialSpeedForDistance(double availableDistance,
|
||
double conservativeInitialAcceleration, double maximumDeceleration,
|
||
double maximumJerk, double directionMaximumSpeed)
|
||
{
|
||
if (!IsFinite(availableDistance) || availableDistance < 0d ||
|
||
!IsFinite(conservativeInitialAcceleration) ||
|
||
!IsPositiveFinite(directionMaximumSpeed))
|
||
throw new ArgumentOutOfRangeException(nameof(availableDistance));
|
||
double lower = 0d;
|
||
double upper = directionMaximumSpeed;
|
||
for (int iteration = 0; iteration < 64; iteration++)
|
||
{
|
||
double candidate = 0.5d * (lower + upper);
|
||
bool fits = TryCalculate(candidate, conservativeInitialAcceleration,
|
||
maximumDeceleration, maximumJerk,
|
||
out JerkLimitedStoppingProfile stop, out _) &&
|
||
stop.DistanceMeters <= availableDistance + NumericTolerance;
|
||
if (fits) lower = candidate;
|
||
else upper = candidate;
|
||
}
|
||
return lower;
|
||
}
|
||
|
||
public static double CalculateMaximumStoppedDistance(double initialSpeed,
|
||
double initialAcceleration, double maximumSpeed, double maximumAcceleration,
|
||
double maximumDeceleration, double maximumJerk, double timeHorizon)
|
||
{
|
||
if (!IsFinite(initialSpeed) || initialSpeed < 0d ||
|
||
!IsFinite(initialAcceleration) || !IsPositiveFinite(maximumSpeed) ||
|
||
!IsPositiveFinite(maximumAcceleration) ||
|
||
!IsPositiveFinite(maximumDeceleration) ||
|
||
!IsPositiveFinite(maximumJerk) || !IsPositiveFinite(timeHorizon))
|
||
throw new ArgumentOutOfRangeException(nameof(timeHorizon));
|
||
double lower = 0d;
|
||
double upper = timeHorizon;
|
||
double bestDistance = 0d;
|
||
for (int iteration = 0; iteration < 64; iteration++)
|
||
{
|
||
double driveDuration = 0.5d * (lower + upper);
|
||
AdvanceTowardMaximumSpeed(initialSpeed, initialAcceleration,
|
||
maximumSpeed, maximumAcceleration, maximumJerk, driveDuration,
|
||
out double driveDistance, out double speed, out double acceleration);
|
||
bool fits = TryCalculate(speed, acceleration, maximumDeceleration,
|
||
maximumJerk, out JerkLimitedStoppingProfile stop, out _) &&
|
||
driveDuration + stop.DurationSeconds <= timeHorizon + NumericTolerance;
|
||
if (fits)
|
||
{
|
||
lower = driveDuration;
|
||
bestDistance = Math.Max(bestDistance, driveDistance + stop.DistanceMeters);
|
||
}
|
||
else
|
||
{
|
||
upper = driveDuration;
|
||
}
|
||
}
|
||
return bestDistance;
|
||
}
|
||
|
||
private static void AdvanceTowardMaximumSpeed(double initialSpeed,
|
||
double initialAcceleration, double maximumSpeed, double maximumAcceleration,
|
||
double maximumJerk, double duration, out double distance,
|
||
out double speed, out double acceleration)
|
||
{
|
||
distance = 0d;
|
||
speed = initialSpeed;
|
||
acceleration = initialAcceleration;
|
||
double remaining = duration;
|
||
while (remaining > NumericTolerance)
|
||
{
|
||
double dt = Math.Min(0.001d, remaining);
|
||
double speedNeededToReleaseAcceleration = acceleration > 0d
|
||
? acceleration * acceleration / (2d * maximumJerk)
|
||
: 0d;
|
||
double jerk = speed + speedNeededToReleaseAcceleration >= maximumSpeed
|
||
? (acceleration > 0d ? -maximumJerk : 0d)
|
||
: (acceleration < maximumAcceleration ? maximumJerk : 0d);
|
||
Integrate(ref distance, ref speed, ref acceleration, jerk, dt);
|
||
if (speed > maximumSpeed && speed - maximumSpeed <= 1e-6d)
|
||
{
|
||
speed = maximumSpeed;
|
||
acceleration = 0d;
|
||
}
|
||
remaining -= dt;
|
||
}
|
||
}
|
||
|
||
private static void Integrate(ref double s, ref double u, ref double a,
|
||
double jerk, double duration)
|
||
{
|
||
s += u * duration + 0.5d * a * duration * duration +
|
||
jerk * duration * duration * duration / 6d;
|
||
u += a * duration + 0.5d * jerk * duration * duration;
|
||
a += jerk * duration;
|
||
}
|
||
|
||
private static bool IsPositiveFinite(double value)
|
||
{
|
||
return IsFinite(value) && value > 0d;
|
||
}
|
||
|
||
private static bool IsFinite(double value)
|
||
{
|
||
return !double.IsNaN(value) && !double.IsInfinity(value);
|
||
}
|
||
}
|
||
```
|
||
|
||
按上述方法实现；不保留 `PathSpeedLimitBuilder.cs` 文件末尾旧的 `LongitudinalStoppingProfile` 和 `LongitudinalStoppingMath`，避免同名实现并存。
|
||
|
||
- [ ] **Step 4: 运行纵向模型测试并确认通过**
|
||
|
||
Run:
|
||
|
||
```powershell
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model
|
||
```
|
||
|
||
Expected: `PASS longitudinal-model`。
|
||
|
||
- [ ] **Step 5: 独立提交停车数学**
|
||
|
||
```powershell
|
||
git add -- ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/JerkLimitedStoppingMath.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs
|
||
git commit -m "feat: add complete jerk-limited stopping math"
|
||
```
|
||
|
||
---
|
||
|
||
### Task 2: 纵向模式、空间窗口和输入契约
|
||
|
||
**Files:**
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmLongitudinalMode.cs`
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalTerminalSchedule.cs`
|
||
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/PlanningHorizonSelector.cs`
|
||
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanningInput.cs`
|
||
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmPlanningRequestValidator.cs`
|
||
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs`
|
||
- Test: `ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs`
|
||
- Test: `ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs`
|
||
|
||
**Interfaces:**
|
||
- Consumes: Task 1 `JerkLimitedStoppingMath`。
|
||
- Produces: `EmLongitudinalMode`、`LongitudinalTerminalSchedule.GetStabilizationStartIndex(IReadOnlyList<double>, double)`、扩展后的 `PlanningHorizonSelection`、扩展后的 `LongitudinalPlanningInput`。
|
||
- Consumers: Tasks 3–8。
|
||
|
||
- [ ] **Step 1: 写出三种模式和固定空间窗口测试**
|
||
|
||
替换 `VerifiesReferenceHorizonSelectionKeepsTheCurrentSegmentBoundary()` 的断言并拆成三个场景：
|
||
|
||
```csharp
|
||
private static void VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime()
|
||
{
|
||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||
configuration.Scheduling.DistanceHorizonMeters = 1d;
|
||
configuration.Scheduling.TimeHorizonSeconds = 2d;
|
||
|
||
DirectionSegmentView longSegment = CreateSegment(4d, EmBoundaryType.Goal);
|
||
EmPlanningStatus status = new PlanningHorizonSelector().Select(
|
||
longSegment, 0d, 0d, 0d, configuration,
|
||
out PlanningHorizonSelection rolling, out string failure);
|
||
Verification.Equal(EmPlanningStatus.Success, status, "rolling selection: " + failure);
|
||
Verification.NearlyEqual(1d, rolling.WindowEndReferenceS,
|
||
"distance horizon defines the LS window");
|
||
Verification.Equal(EmLongitudinalMode.RollingContinuation,
|
||
rolling.LongitudinalMode, "far boundary rolls");
|
||
|
||
DirectionSegmentView visibleButFar = CreateSegment(0.35d, EmBoundaryType.Goal);
|
||
status = new PlanningHorizonSelector().Select(
|
||
visibleButFar, 0d, 0d, 0d, configuration,
|
||
out PlanningHorizonSelection approach, out failure);
|
||
Verification.Equal(EmPlanningStatus.Success, status, "approach selection: " + failure);
|
||
Verification.Equal(EmLongitudinalMode.ApproachStopBoundary,
|
||
approach.LongitudinalMode, "visible unreachable boundary approaches");
|
||
|
||
DirectionSegmentView reachableGoal = CreateSegment(0.10d, EmBoundaryType.Goal);
|
||
status = new PlanningHorizonSelector().Select(
|
||
reachableGoal, 0d, 0d, 0d, configuration,
|
||
out PlanningHorizonSelection exact, out failure);
|
||
Verification.Equal(EmPlanningStatus.Success, status, "exact selection: " + failure);
|
||
Verification.Equal(EmLongitudinalMode.ExactStopAtBoundary,
|
||
exact.LongitudinalMode, "reachable goal stops exactly");
|
||
Verification.Equal(EmTerminalType.Goal, exact.TerminalType,
|
||
"exact stop preserves Goal identity");
|
||
|
||
IReadOnlyList<double> regularTimes = LongitudinalCandidate.CreateKnotTimes(2d, 0.1d);
|
||
Verification.Equal(19, LongitudinalTerminalSchedule.GetStabilizationStartIndex(
|
||
regularTimes, 0.1d), "regular exact stop reserves t=1.9..2.0");
|
||
Verification.Equal(1, LongitudinalTerminalSchedule.GetStabilizationStartIndex(
|
||
new[] { 0d, 0.1d, 0.2d, 0.25d }, 0.1d),
|
||
"short final interval moves the stop anchor earlier");
|
||
}
|
||
```
|
||
|
||
在 `FoundationChecks` 添加配置失败场景，设置 `DistanceHorizonMeters` 小于“最大状态停车距离 + 一周期行驶余量”，断言 `InvalidInput` 且原因包含 `DistanceHorizonMeters`、`required` 和 `configured`。
|
||
|
||
- [ ] **Step 2: 运行失败测试**
|
||
|
||
Run:
|
||
|
||
```powershell
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- foundation
|
||
```
|
||
|
||
Expected: 编译失败，指出新枚举和选择结果属性不存在；旧选择器也不会返回三种预期模式。
|
||
|
||
- [ ] **Step 3: 增加模式和选择结果**
|
||
|
||
创建：
|
||
|
||
```csharp
|
||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||
|
||
public enum EmLongitudinalMode
|
||
{
|
||
RollingContinuation,
|
||
ApproachStopBoundary,
|
||
ExactStopAtBoundary,
|
||
}
|
||
```
|
||
|
||
将 `PlanningHorizonSelection` 定义调整为：
|
||
|
||
```csharp
|
||
internal PlanningHorizonSelection(double windowEndReferenceS,
|
||
EmTerminalType terminalType, EmLongitudinalMode longitudinalMode,
|
||
double stopBoundaryReferenceS, bool hasStopBoundary)
|
||
{
|
||
WindowEndReferenceS = windowEndReferenceS;
|
||
TerminalType = terminalType;
|
||
LongitudinalMode = longitudinalMode;
|
||
StopBoundaryReferenceS = stopBoundaryReferenceS;
|
||
HasStopBoundary = hasStopBoundary;
|
||
}
|
||
|
||
public double WindowEndReferenceS { get; }
|
||
public double TerminalReferenceS => WindowEndReferenceS;
|
||
public EmTerminalType TerminalType { get; }
|
||
public EmLongitudinalMode LongitudinalMode { get; }
|
||
public double StopBoundaryReferenceS { get; }
|
||
public bool HasStopBoundary { get; }
|
||
```
|
||
|
||
保留只读 `TerminalReferenceS` 兼容别名，避免同时修改不相关调用方；新代码只使用 `WindowEndReferenceS`。
|
||
|
||
同时创建统一稳定尾选择器：
|
||
|
||
```csharp
|
||
public static class LongitudinalTerminalSchedule
|
||
{
|
||
public static int GetStabilizationStartIndex(IReadOnlyList<double> knotTimes,
|
||
double minimumStabilizationDurationSeconds)
|
||
{
|
||
if (knotTimes == null || knotTimes.Count < 3 ||
|
||
double.IsNaN(minimumStabilizationDurationSeconds) ||
|
||
double.IsInfinity(minimumStabilizationDurationSeconds) ||
|
||
minimumStabilizationDurationSeconds <= 0d)
|
||
throw new ArgumentException("A positive stabilization tail and at least three knots are required.");
|
||
double previous = double.NegativeInfinity;
|
||
for (int index = 0; index < knotTimes.Count; index++)
|
||
{
|
||
if (double.IsNaN(knotTimes[index]) || double.IsInfinity(knotTimes[index]) ||
|
||
knotTimes[index] <= previous)
|
||
throw new ArgumentException("Terminal-schedule knots must be finite and strictly increasing.");
|
||
previous = knotTimes[index];
|
||
}
|
||
double finalTime = knotTimes[knotTimes.Count - 1];
|
||
for (int index = knotTimes.Count - 2; index >= 1; index--)
|
||
{
|
||
if (finalTime - knotTimes[index] >= minimumStabilizationDurationSeconds - 1e-12d)
|
||
return index;
|
||
}
|
||
throw new ArgumentException("The time horizon cannot contain a full terminal stabilization interval.");
|
||
}
|
||
}
|
||
```
|
||
|
||
- [ ] **Step 4: 重写窗口选择器**
|
||
|
||
核心分支必须等价于：
|
||
|
||
```csharp
|
||
double remaining = segment.LengthMeters - currentSegmentReferenceS;
|
||
if (!JerkLimitedStoppingMath.TryCalculate(initialSpeed, initialAcceleration,
|
||
longitudinal.MaximumDecelerationMetersPerSecondSquared,
|
||
longitudinal.MaximumJerkMetersPerSecondCubed,
|
||
out JerkLimitedStoppingProfile stop, out failureReason))
|
||
return EmPlanningStatus.StoppingDistanceInsufficient;
|
||
|
||
if (stop.DistanceMeters > remaining + BoundaryTolerance)
|
||
{
|
||
failureReason = "StoppingDistanceInsufficient: remaining=" + remaining +
|
||
";required=" + stop.DistanceMeters +
|
||
";shortage=" + (stop.DistanceMeters - remaining);
|
||
return EmPlanningStatus.StoppingDistanceInsufficient;
|
||
}
|
||
|
||
double windowEnd = Math.Min(currentSegmentReferenceS +
|
||
scheduling.DistanceHorizonMeters, segment.LengthMeters);
|
||
bool containsBoundary = windowEnd >= segment.LengthMeters - BoundaryTolerance;
|
||
if (!containsBoundary)
|
||
{
|
||
selection = new PlanningHorizonSelection(windowEnd,
|
||
EmTerminalType.RollingSafetyStop,
|
||
EmLongitudinalMode.RollingContinuation,
|
||
segment.LengthMeters, false);
|
||
return EmPlanningStatus.Success;
|
||
}
|
||
|
||
IReadOnlyList<double> knotTimes = LongitudinalCandidate.CreateKnotTimes(
|
||
scheduling.TimeHorizonSeconds, scheduling.OutputTimeStepSeconds);
|
||
int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
|
||
knotTimes, scheduling.OutputTimeStepSeconds);
|
||
double availableMotionTime = knotTimes[stabilizationStart];
|
||
double maximumStoppedDistance =
|
||
JerkLimitedStoppingMath.CalculateMaximumStoppedDistance(
|
||
initialSpeed, initialAcceleration, directionMaximum,
|
||
longitudinal.MaximumAccelerationMetersPerSecondSquared,
|
||
longitudinal.MaximumDecelerationMetersPerSecondSquared,
|
||
longitudinal.MaximumJerkMetersPerSecondCubed,
|
||
availableMotionTime);
|
||
EmLongitudinalMode mode = remaining <= maximumStoppedDistance + BoundaryTolerance
|
||
? EmLongitudinalMode.ExactStopAtBoundary
|
||
: EmLongitudinalMode.ApproachStopBoundary;
|
||
selection = new PlanningHorizonSelection(segment.LengthMeters,
|
||
ToTerminalType(segment.EndBoundary.BoundaryType), mode,
|
||
segment.LengthMeters, true);
|
||
return EmPlanningStatus.Success;
|
||
```
|
||
|
||
删除旧 `CalculateReachableDistance()` 中扣除 `ZeroSpeedHoldSeconds` 的逻辑。
|
||
|
||
- [ ] **Step 5: 扩展纵向输入并验证组合**
|
||
|
||
给 `LongitudinalPlanningInput` 构造函数增加 `EmLongitudinalMode mode`，并增加：
|
||
|
||
```csharp
|
||
public EmLongitudinalMode Mode { get; }
|
||
public double PathUpperBoundS => Path.Points[Path.Points.Count - 1].PathS;
|
||
public bool HasStopBoundary => TerminalType != EmTerminalType.RollingSafetyStop;
|
||
public double StopBoundaryPathS => HasStopBoundary ? PathUpperBoundS : double.NaN;
|
||
[Obsolete("Use PathUpperBoundS or StopBoundaryPathS explicitly.")]
|
||
public double TerminalPathS => PathUpperBoundS;
|
||
```
|
||
|
||
构造函数拒绝以下矛盾组合：
|
||
|
||
```csharp
|
||
if (mode == EmLongitudinalMode.RollingContinuation &&
|
||
terminalType != EmTerminalType.RollingSafetyStop)
|
||
throw new ArgumentException("Rolling continuation requires a rolling window boundary.");
|
||
if (mode != EmLongitudinalMode.RollingContinuation &&
|
||
terminalType == EmTerminalType.RollingSafetyStop)
|
||
throw new ArgumentException("Stop-boundary modes require Goal or GearSwitch.");
|
||
```
|
||
|
||
- [ ] **Step 6: 增加前视停车余量配置验证**
|
||
|
||
在 `EmPlanningRequestValidator.TryValidateConfiguration` 中计算正向和反向最大速度下的保守停车距离，取较大值并增加：
|
||
|
||
```text
|
||
previewReserve = maxSpeed * ReplanPeriodSeconds
|
||
requiredDistanceHorizon = worstStoppingDistance + previewReserve
|
||
```
|
||
|
||
若配置不足，返回包含配置值和所需值的具体原因。配置验证还调用 `LongitudinalTerminalSchedule.GetStabilizationStartIndex`，要求预留一个完整 `OutputTimeStepSeconds` 稳定尾之后仍至少有一个正运动 interval；停车时间超过剩余运动时间由 `ApproachStopBoundary` 处理，不拒绝配置。
|
||
|
||
- [ ] **Step 7: 更新所有 `LongitudinalPlanningInput` 构造调用并运行测试**
|
||
|
||
所有现有 Goal/GearSwitch 测试先传 `EmLongitudinalMode.ExactStopAtBoundary`；原滚动测试传 `RollingContinuation`。`EmPlanningService` 创建纵向输入时传 `horizon.LongitudinalMode`，切片上界改用 `horizon.WindowEndReferenceS`。随后运行：
|
||
|
||
```powershell
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- foundation
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model
|
||
```
|
||
|
||
Expected: `PASS foundation` 和 `PASS longitudinal-model`。
|
||
|
||
- [ ] **Step 8: 独立提交模式与窗口语义**
|
||
|
||
```powershell
|
||
git add -- ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmLongitudinalMode.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalTerminalSchedule.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Segmentation/PlanningHorizonSelector.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPlanningInput.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmPlanningRequestValidator.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs ClumsyPilot/tests/EMPlannerVerificationHost/FoundationChecks.cs
|
||
git commit -m "feat: separate rolling horizons from stop boundaries"
|
||
```
|
||
|
||
---
|
||
|
||
### Task 3: 模式化 PathS 速度包络
|
||
|
||
**Files:**
|
||
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimit.cs`
|
||
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs`
|
||
- Test: `ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs`
|
||
|
||
**Interfaces:**
|
||
- Consumes: Task 1 停车距离反求；Task 2 `LongitudinalPlanningInput.Mode`。
|
||
- Produces: 末端可为非零的 `PathSpeedLimit`，以及面向真实边界的停止速度上限。
|
||
- Consumers: Tasks 4、5。
|
||
|
||
- [ ] **Step 1: 写出滚动末端非零和真实边界零速测试**
|
||
|
||
加入：
|
||
|
||
```csharp
|
||
private static void VerifiesRollingEnvelopeDoesNotStopAtWindowEnd()
|
||
{
|
||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||
LateralPath path = CreatePath(new[]
|
||
{
|
||
new PathFixture(0d, 0d, 0d, 0d),
|
||
new PathFixture(1d, 1d, 0d, 0d),
|
||
});
|
||
var rolling = new LongitudinalPlanningInput(path, TravelDirection.Forward,
|
||
0d, 0d, EmTerminalType.RollingSafetyStop,
|
||
EmLongitudinalMode.RollingContinuation, configuration,
|
||
Array.Empty<double>(), Array.Empty<double>());
|
||
EmPlanningStatus status = new PathSpeedLimitBuilder().Build(
|
||
rolling, out PathSpeedLimit envelope, out string failure);
|
||
Verification.Equal(EmPlanningStatus.Success, status, "rolling envelope: " + failure);
|
||
Verification.True(envelope.MaximumSpeedAt(rolling.PathUpperBoundS) > 0d,
|
||
"rolling window end keeps a nonzero speed allowance");
|
||
Verification.True(!envelope.HasStopBoundary,
|
||
"rolling envelope has no stop boundary");
|
||
}
|
||
```
|
||
|
||
保留 Goal 测试，但改为 `ExactStopAtBoundary` 并断言真实边界速度严格为零。增加 `ApproachStopBoundary` 测试，断言末端停止上限为零、时间末点不由该类固定。
|
||
|
||
- [ ] **Step 2: 运行测试并确认滚动末端仍被置零**
|
||
|
||
```powershell
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model
|
||
```
|
||
|
||
Expected: `rolling window end keeps a nonzero speed allowance` 失败。
|
||
|
||
- [ ] **Step 3: 修改 `PathSpeedLimit` 契约**
|
||
|
||
构造函数增加 `bool hasStopBoundary`，移除无条件末项为零校验，改为：
|
||
|
||
```csharp
|
||
if (hasStopBoundary &&
|
||
(MaximumSpeedMetersPerSecond[MaximumSpeedMetersPerSecond.Count - 1] != 0d ||
|
||
StoppingSpeedLimitsMetersPerSecond[StoppingSpeedLimitsMetersPerSecond.Count - 1] != 0d))
|
||
throw new ArgumentException("A real stop boundary must have an exact zero speed limit.");
|
||
|
||
HasStopBoundary = hasStopBoundary;
|
||
```
|
||
|
||
公开：
|
||
|
||
```csharp
|
||
public bool HasStopBoundary { get; }
|
||
public double PathUpperBoundS => PathS[PathS.Count - 1];
|
||
[Obsolete("Use PathUpperBoundS.")]
|
||
public double TerminalPathS => PathUpperBoundS;
|
||
```
|
||
|
||
- [ ] **Step 4: 按模式构建停止速度上限**
|
||
|
||
`RollingContinuation` 的 `stopping` 数组全部填 `directionMaximum`，最终组合速度不再因局部窗口结束而变成零。
|
||
|
||
`ApproachStopBoundary` 和 `ExactStopAtBoundary` 对每个 sample 使用：
|
||
|
||
```csharp
|
||
double remaining = input.StopBoundaryPathS - samplePathS;
|
||
double stoppingValue = JerkLimitedStoppingMath.MaximumInitialSpeedForDistance(
|
||
Math.Max(0d, remaining), maximumAcceleration,
|
||
maximumDeceleration, maximumJerk, directionMaximum);
|
||
```
|
||
|
||
真实边界采样严格赋值零。删除旧的 `sqrt(2d * maximumDeceleration * remaining)` 和旧离散减速尾部公式；采样加密站根据新停车模型的距离反求点生成。
|
||
|
||
- [ ] **Step 5: 运行测试并提交**
|
||
|
||
```powershell
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model
|
||
git add -- ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimit.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/PathSpeedLimitBuilder.cs ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs
|
||
git commit -m "feat: keep rolling speed envelopes open"
|
||
```
|
||
|
||
Expected: `PASS longitudinal-model`。
|
||
|
||
---
|
||
|
||
### Task 4: 条件化 ST QP 终端约束与独立验证
|
||
|
||
**Files:**
|
||
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs`
|
||
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs`
|
||
- Test: `ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs`
|
||
|
||
**Interfaces:**
|
||
- Consumes: `LongitudinalPlanningInput.Mode`、`PathUpperBoundS`、`StopBoundaryPathS`。
|
||
- Produces: 滚动/接近无终端等式、准确停车对边界锚点和内部稳定尾逐 knot 固定 `S/U/A` 的 QP。
|
||
- Consumers: Task 5 optimizer。
|
||
|
||
- [ ] **Step 1: 扩展约束结构测试**
|
||
|
||
在现有 `VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints` 中分别构建滚动和准确停车输入并断言：
|
||
|
||
```csharp
|
||
Verification.Equal(0, CountExactEqualityRows(rollingProblem,
|
||
new Dictionary<int, double> { { layout.S(4), 1d } }, rollingInput.PathUpperBoundS),
|
||
"rolling has no exact terminal S");
|
||
Verification.Equal(0, CountExactEqualityRows(rollingProblem,
|
||
new Dictionary<int, double> { { layout.U(4), 1d } }, 0d),
|
||
"rolling has no exact terminal U");
|
||
Verification.Equal(0, CountExactEqualityRows(rollingProblem,
|
||
new Dictionary<int, double> { { layout.A(4), 1d } }, 0d),
|
||
"rolling has no exact terminal A");
|
||
|
||
int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
|
||
integrated.KnotTimes, configuration.Scheduling.OutputTimeStepSeconds);
|
||
for (int index = stabilizationStart; index < layout.KnotCount; index++)
|
||
{
|
||
Verification.Equal(1, CountExactEqualityRows(stopProblem,
|
||
new Dictionary<int, double> { { layout.S(index), 1d } }, stopInput.StopBoundaryPathS),
|
||
"stop tail exact S " + index);
|
||
Verification.Equal(1, CountExactEqualityRows(stopProblem,
|
||
new Dictionary<int, double> { { layout.U(index), 1d } }, 0d),
|
||
"stop tail exact U " + index);
|
||
Verification.Equal(1, CountExactEqualityRows(stopProblem,
|
||
new Dictionary<int, double> { { layout.A(index), 1d } }, 0d),
|
||
"stop tail exact A " + index);
|
||
}
|
||
```
|
||
|
||
增加验证器测试：滚动候选末速非零应通过；准确停车候选在稳定尾任一 knot 出现 `S/U/A` 偏差应失败；Approach 候选任一点违反 `S + stoppingDistance(U,A) <= StopBoundaryPathS` 应失败。
|
||
|
||
- [ ] **Step 2: 运行失败测试**
|
||
|
||
```powershell
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model
|
||
```
|
||
|
||
Expected: 旧 QP 仍为所有模式添加终端 `S/U`，且没有终端 `A`，测试失败。
|
||
|
||
- [ ] **Step 3: 动态计算约束行并条件化终端约束**
|
||
|
||
在 `TryBuild` 中使用：
|
||
|
||
```csharp
|
||
int stabilizationStart = input.Mode == EmLongitudinalMode.ExactStopAtBoundary
|
||
? LongitudinalTerminalSchedule.GetStabilizationStartIndex(expectedTimes,
|
||
input.Configuration.Scheduling.OutputTimeStepSeconds)
|
||
: layout.KnotCount;
|
||
int stationaryKnotCount = layout.KnotCount - stabilizationStart;
|
||
int terminalRows = 3 * stationaryKnotCount;
|
||
int expectedRows = 8 * layout.KnotCount - 2 + terminalRows;
|
||
var constraints = new SparseTripletBuilder(expectedRows, layout.VariableCount);
|
||
var lower = new List<double>(expectedRows);
|
||
var upper = new List<double>(expectedRows);
|
||
```
|
||
|
||
将 `AddExactStartAndTerminal` 拆成：
|
||
|
||
```csharp
|
||
AddExactStart(input, layout, constraints, lower, upper, ref row);
|
||
if (input.Mode == EmLongitudinalMode.ExactStopAtBoundary)
|
||
AddExactStopTail(input, layout, stabilizationStart,
|
||
constraints, lower, upper, ref row);
|
||
```
|
||
|
||
`AddExactStopTail` 对 `stabilizationStart..KnotCount-1` 的每个 knot 依次添加 `S=StopBoundaryPathS`、`U=0`、`A=0` 三个等式。
|
||
|
||
- [ ] **Step 4: 条件化候选验证和规范化**
|
||
|
||
始终验证普通范围和动力学。仅准确停车执行：
|
||
|
||
```csharp
|
||
int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
|
||
candidate.KnotTimes, input.Configuration.Scheduling.OutputTimeStepSeconds);
|
||
for (int index = stabilizationStart; index < candidate.S.Count; index++)
|
||
{
|
||
if (!AreClose(candidate.S[index], input.StopBoundaryPathS, tolerance) ||
|
||
!AreClose(candidate.U[index], 0d, tolerance) ||
|
||
!AreClose(candidate.A[index], 0d, tolerance))
|
||
{
|
||
failureReason = "ST candidate does not satisfy the exact stabilized S/U/A stop tail at knot " + index + ".";
|
||
return false;
|
||
}
|
||
}
|
||
```
|
||
|
||
Approach 和 ExactStop 逐点执行：
|
||
|
||
```csharp
|
||
if (!JerkLimitedStoppingMath.TryCalculate(candidate.U[index], candidate.A[index],
|
||
maximumDeceleration, maximumJerk, out JerkLimitedStoppingProfile stop, out _) ||
|
||
candidate.S[index] + stop.DistanceMeters > input.StopBoundaryPathS + tolerance)
|
||
{
|
||
failureReason = "ST candidate leaves the jerk-limited stoppable set at knot " + index + ".";
|
||
return false;
|
||
}
|
||
```
|
||
|
||
只在准确停车模式将 canonical 的 `stabilizationStart..last` 全部写成 `StopBoundaryPathS/0/0`。
|
||
|
||
- [ ] **Step 5: 运行测试并提交**
|
||
|
||
```powershell
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model
|
||
git add -- ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalConstraintBuilder.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs
|
||
git commit -m "feat: apply ST stop constraints only at real boundaries"
|
||
```
|
||
|
||
Expected: `PASS longitudinal-model`。
|
||
|
||
---
|
||
|
||
### Task 5: 按模式构造纵向初始解和外层迭代
|
||
|
||
**Files:**
|
||
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs`
|
||
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalObjectiveBuilder.cs`
|
||
- Test: `ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs`
|
||
|
||
**Interfaces:**
|
||
- Consumes: Tasks 1–4 的停车模型、速度包络和条件化约束。
|
||
- Produces: 可供 OSQP 热启动的滚动、接近和准确停车候选。
|
||
|
||
- [ ] **Step 1: 写出滚动初始解和 point-21 回归测试**
|
||
|
||
在 `LongitudinalIntegrationChecks.Run()` 增加：
|
||
|
||
```csharp
|
||
VerifiesRollingOptimizationKeepsANonzeroTerminalSpeed();
|
||
VerifiesExactStopIncludesAStabilizationTail();
|
||
```
|
||
|
||
滚动测试使用直线 `LateralPath`、`TimeHorizonSeconds=2d`、`OutputTimeStepSeconds=0.1d`、`PathUpperBoundS=5d`，由现有 fake solver 返回优化器初始/严格 fallback，断言：
|
||
|
||
```csharp
|
||
Verification.True(result.Candidate.S[result.Candidate.S.Count - 1] < input.PathUpperBoundS,
|
||
"two-second ST does not consume the five-metre LS window");
|
||
Verification.True(result.Candidate.U[result.Candidate.U.Count - 1] > 0d,
|
||
"rolling ST keeps nonzero terminal speed");
|
||
```
|
||
|
||
准确停车测试计算 `stabilizationStart` 并断言整个内部稳定尾：
|
||
|
||
```csharp
|
||
int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
|
||
result.Candidate.KnotTimes, input.Configuration.Scheduling.OutputTimeStepSeconds);
|
||
for (int index = stabilizationStart; index < result.Candidate.S.Count; index++)
|
||
{
|
||
Verification.NearlyEqual(input.StopBoundaryPathS, result.Candidate.S[index],
|
||
"stop-tail S " + index);
|
||
Verification.NearlyEqual(0d, result.Candidate.U[index], "stop-tail U " + index);
|
||
Verification.NearlyEqual(0d, result.Candidate.A[index], "stop-tail A " + index);
|
||
}
|
||
```
|
||
|
||
- [ ] **Step 2: 运行失败测试**
|
||
|
||
```powershell
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-integration
|
||
```
|
||
|
||
Expected: 滚动候选仍被强制到路径末端零速，测试失败。
|
||
|
||
- [ ] **Step 3: 将初始迭代分派到三个构造器**
|
||
|
||
调用点先改为 `CreateInitialIterate(input, speedLimit)`，方法按模式分派：
|
||
|
||
```csharp
|
||
switch (input.Mode)
|
||
{
|
||
case EmLongitudinalMode.RollingContinuation:
|
||
return CreateRollingSeed(input, times, speedLimit);
|
||
case EmLongitudinalMode.ApproachStopBoundary:
|
||
return CreateApproachSeed(input, times, speedLimit);
|
||
case EmLongitudinalMode.ExactStopAtBoundary:
|
||
return CreateExactStopSeed(input, times);
|
||
default:
|
||
throw new ArgumentOutOfRangeException(nameof(input.Mode));
|
||
}
|
||
```
|
||
|
||
`CreateRollingSeed` 从 `S0=0,U0,A0` 开始，在每个 interval 内选择使速度向 `speedLimit.MaximumSpeedAt(S[k])` 靠近的 jerk，并使用 `LongitudinalCandidate.Integrate` 相同公式积分；遇到速度、加速度或 PathUpperBoundS 时缩小 jerk，不写死末项。
|
||
|
||
`CreateApproachSeed` 生成“允许巡航段 + 完整 jerk 受限停车段”，然后仅保留 `TimeHorizonSeconds` 内的前缀；每个 knot 的参考速度不超过 stopping envelope。
|
||
|
||
`CreateExactStopSeed` 先通过 `LongitudinalTerminalSchedule` 得到 `stabilizationStart`，只使用 `times[0..stabilizationStart]` 安排受限制的加速/巡航和 Task 1 三阶段停车，使 `stabilizationStart` 准确达到 `S_end,U=0,A=0`。随后将其余 knot 填充为同一 `S_end` 上的 `U=0,A=0,J=0` 内部稳定尾。
|
||
|
||
- [ ] **Step 4: 移除外层迭代中的无条件末点固定**
|
||
|
||
`TryCreateEnvelopeIterate` 中只在准确停车模式执行：
|
||
|
||
```csharp
|
||
int stabilizationStart = input.Mode == EmLongitudinalMode.ExactStopAtBoundary
|
||
? LongitudinalTerminalSchedule.GetStabilizationStartIndex(candidate.KnotTimes,
|
||
input.Configuration.Scheduling.OutputTimeStepSeconds)
|
||
: candidate.S.Count;
|
||
if (input.Mode == EmLongitudinalMode.ExactStopAtBoundary &&
|
||
index >= stabilizationStart)
|
||
candidateProgress = input.StopBoundaryPathS;
|
||
```
|
||
|
||
所有对 `input.TerminalPathS` 的上界用途改为 `PathUpperBoundS`。终点用途改为 `StopBoundaryPathS`，并且必须位于准确停车分支中。
|
||
|
||
`LongitudinalObjectiveBuilder` 保留速度、加速度、jerk 和上一轨迹目标；`TerminalAcceleration` 软目标只对非准确停车模式添加，准确停车的锚点和内部稳定尾已经由硬约束保证 `A=0`。
|
||
|
||
- [ ] **Step 5: 运行纵向模型与集成测试并提交**
|
||
|
||
```powershell
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-integration
|
||
git add -- ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/SequentialLongitudinalOptimizer.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalObjectiveBuilder.cs ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs
|
||
git commit -m "feat: seed rolling and exact-stop ST profiles"
|
||
```
|
||
|
||
Expected: `PASS longitudinal-model`、`PASS longitudinal-integration`。
|
||
|
||
---
|
||
|
||
### Task 6: 条件化轨迹采样、边界锚点和发布验证
|
||
|
||
**Files:**
|
||
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectoryMetadata.cs`
|
||
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs`
|
||
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs`
|
||
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmTrajectoryValidator.cs`
|
||
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs`
|
||
- Test: `ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs`
|
||
- Test: `ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs`
|
||
- Test: `ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs`
|
||
- Test: `ClumsyPilot/tests/EMPlannerVerificationHost/CoordinatorChecks.cs`
|
||
|
||
**Interfaces:**
|
||
- Consumes: `EmLongitudinalMode` 和经过验证的纵向候选。
|
||
- Produces: 滚动轨迹不带保持点；准确停车轨迹带安全保持点和真实边界锚点。
|
||
|
||
- [ ] **Step 1: 写出滚动采样和终端 jerk 回归测试**
|
||
|
||
在 `TrajectoryChecks.Run()` 增加：
|
||
|
||
```csharp
|
||
VerifiesRollingTrajectoryHasNoSyntheticStopTail();
|
||
VerifiesExactStopHoldHasNoJerkDiscontinuity();
|
||
```
|
||
|
||
滚动测试使用 21 个 `0.1s` knot、非零末速和非零但受限的末加速度，断言点数仍为 21、末点 `BoundaryType.None`、末速非零。
|
||
|
||
准确停车回归构造的速度有限差分状态：
|
||
|
||
```text
|
||
t=1.8: U=0.0025, A=-0.05
|
||
t=1.9: U=0, A=0, exact stop anchor
|
||
t=2.0: U=0, A=0, QP stabilization point
|
||
t=2.1: U=0, A=0, first external hold
|
||
```
|
||
|
||
然后运行 `EmTrajectoryValidator`，断言通过，并显式计算：
|
||
|
||
```csharp
|
||
double terminalFiniteDifferenceAcceleration =
|
||
(anchor.SignedLongitudinalVelocity - previousMoving.SignedLongitudinalVelocity) /
|
||
(anchor.TimeFromStart - previousMoving.TimeFromStart);
|
||
double stabilizationAcceleration =
|
||
(stabilization.SignedLongitudinalVelocity - anchor.SignedLongitudinalVelocity) /
|
||
(stabilization.TimeFromStart - anchor.TimeFromStart);
|
||
double externalHoldAcceleration =
|
||
(firstExternalHold.SignedLongitudinalVelocity - stabilization.SignedLongitudinalVelocity) /
|
||
(firstExternalHold.TimeFromStart - stabilization.TimeFromStart);
|
||
double jerkIntoStabilization = (stabilizationAcceleration -
|
||
terminalFiniteDifferenceAcceleration) /
|
||
(stabilization.TimeFromStart - anchor.TimeFromStart);
|
||
double jerkIntoExternalHold = (externalHoldAcceleration - stabilizationAcceleration) /
|
||
(firstExternalHold.TimeFromStart - stabilization.TimeFromStart);
|
||
Verification.True(Math.Abs(jerkIntoStabilization) <= 0.5d,
|
||
"jerk into QP stabilization stays within the limit");
|
||
Verification.NearlyEqual(0d, jerkIntoExternalHold,
|
||
"jerk into the external hold is zero");
|
||
```
|
||
|
||
再构造内部稳定尾任一 knot 的 `S/U/A` 非零偏差，断言采样器或发布验证器拒绝；构造真实边界锚点存储加速度非零的轨迹，断言发布验证器以 `TerminalAccelerationNotZero` 拒绝。
|
||
|
||
- [ ] **Step 2: 运行测试并确认旧采样器拒绝滚动末速**
|
||
|
||
```powershell
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory
|
||
```
|
||
|
||
Expected: 旧 `TrajectorySampleSchedule` 抛出“requires an exact zero-speed terminal candidate”或点数/边界断言失败。
|
||
|
||
- [ ] **Step 3: 给元数据增加纵向模式**
|
||
|
||
`EmTrajectoryMetadata` 构造函数末尾增加 `EmLongitudinalMode longitudinalMode`，验证枚举并公开：
|
||
|
||
```csharp
|
||
public EmLongitudinalMode LongitudinalMode { get; }
|
||
```
|
||
|
||
`EmPlanningService` 构造元数据时传 `horizon.LongitudinalMode`。`TrajectoryChecks` 中 Goal/GearSwitch 元数据传 `ExactStopAtBoundary`；`TrajectoryObservationChecks`、`ExecutorChecks` 和 `CoordinatorChecks` 中代表运动中轨迹的 fixture 传 `RollingContinuation`，代表完成停车的 fixture 传 `ExactStopAtBoundary`。
|
||
|
||
- [ ] **Step 4: 按模式构建采样计划**
|
||
|
||
`TrajectorySampleSchedule` 构造函数增加 mode：
|
||
|
||
```csharp
|
||
if (mode != EmLongitudinalMode.ExactStopAtBoundary)
|
||
{
|
||
Samples = new ReadOnlyCollection<TrajectorySample>(candidateSamples);
|
||
TerminalAnchorSampleIndex = -1;
|
||
return;
|
||
}
|
||
int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
|
||
candidate.KnotTimes, outputTimeStepSeconds);
|
||
double stopPathS = candidate.S[stabilizationStart];
|
||
for (int index = stabilizationStart; index < candidate.S.Count; index++)
|
||
{
|
||
if (Math.Abs(candidate.S[index] - stopPathS) > ZeroTolerance ||
|
||
Math.Abs(candidate.U[index]) > ZeroTolerance ||
|
||
Math.Abs(candidate.A[index]) > ZeroTolerance ||
|
||
(index < candidate.J.Count && Math.Abs(candidate.J[index]) > ZeroTolerance))
|
||
throw new ArgumentException("An exact stop requires a stationary S/U/A/J tail.");
|
||
}
|
||
TerminalAnchorSampleIndex = stabilizationStart;
|
||
AppendHoldSamples(candidateSamples, outputTimeStepSeconds, holdDurationSeconds);
|
||
```
|
||
|
||
公开只读 `TerminalAnchorSampleIndex`。保持段从优化末点之后开始，不复制同时间戳点。
|
||
|
||
- [ ] **Step 5: 只为准确停车标记边界锚点**
|
||
|
||
`EmTrajectoryAssembler` 使用：
|
||
|
||
```csharp
|
||
bool isTerminalAnchor = metadata.LongitudinalMode ==
|
||
EmLongitudinalMode.ExactStopAtBoundary &&
|
||
index == schedule.TerminalAnchorSampleIndex;
|
||
EmBoundaryType boundaryType = isTerminalAnchor
|
||
? ToBoundaryType(metadata.TerminalType)
|
||
: EmBoundaryType.None;
|
||
```
|
||
|
||
滚动时间末点和 Approach 时间末点都保持 `BoundaryType.None`。
|
||
|
||
- [ ] **Step 6: 条件化世界坐标发布验证**
|
||
|
||
在 `EmTrajectoryValidationFailure` 增加：
|
||
|
||
```csharp
|
||
TerminalAccelerationNotZero,
|
||
```
|
||
|
||
始终执行普通运动学、碰撞和有限差分 jerk 检查。仅准确停车模式寻找真实边界锚点，检查锚点存储的 `LongitudinalAcceleration`、速度和 yaw rate 近零，并验证从锚点到轨迹末尾保持同一 PathS、零速和零状态加速度。滚动/Approach 不调用 `FindTerminalAnchor`，也不检查零末态。
|
||
|
||
- [ ] **Step 7: 运行轨迹测试并提交**
|
||
|
||
```powershell
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- coordinator
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- executor
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory-observation
|
||
git add -- ClumsyPilot/ParkrobTrajplanner/EMPlanner/Contracts/EmTrajectoryMetadata.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/EmTrajectoryAssembler.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmTrajectoryValidator.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryChecks.cs ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs ClumsyPilot/tests/EMPlannerVerificationHost/CoordinatorChecks.cs
|
||
git commit -m "feat: publish rolling trajectories without stop tails"
|
||
```
|
||
|
||
Expected: `PASS trajectory`、`PASS coordinator`、`PASS executor`、`PASS trajectory-observation`，point-21 回归通过。
|
||
|
||
---
|
||
|
||
### Task 7: 上一轨迹纵向软参考
|
||
|
||
**Files:**
|
||
- Create: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPreviousTrajectorySeedBuilder.cs`
|
||
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs`
|
||
- Test: `ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs`
|
||
- Test: `ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs`
|
||
|
||
**Interfaces:**
|
||
- Produces: `LongitudinalPreviousTrajectorySeed`，包含相同数量的 PathS 和进度速度样本。
|
||
- Consumes: 上一条 `EmTrajectory`、新 `LateralPath`、新轨迹生效时间和新 knot 时间。
|
||
|
||
- [ ] **Step 1: 写出时间对齐、方向过滤和 PathS 映射测试**
|
||
|
||
添加测试：当前 LateralPath 是 `X/PathS=0..1m` 的直线；上一轨迹 `EffectiveAtUtc=10s`，在 `t=0,0.1,0.2,0.3,0.4s` 分别位于 `X=0,0.1,0.2,0.3,0.4m`；以 `newEffectiveAtUtc=10.2s`、新 knot `{0,0.1,0.2}` 重采样。断言：
|
||
|
||
```csharp
|
||
Verification.Equal(3, seed.PathS.Count, "previous seed path-S count");
|
||
Verification.Equal(3, seed.ProgressSpeedMetersPerSecond.Count,
|
||
"previous seed speed count");
|
||
Verification.NearlyEqual(0.20d, seed.PathS[0],
|
||
"previous seed begins at new absolute effective time");
|
||
Verification.True(seed.PathS[1] >= seed.PathS[0] && seed.PathS[2] >= seed.PathS[1],
|
||
"previous seed progress is monotone");
|
||
```
|
||
|
||
再用不同方向、不同 segment 和已超出上一轨迹时间范围的输入，断言返回空 seed。
|
||
|
||
- [ ] **Step 2: 运行失败测试**
|
||
|
||
```powershell
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model
|
||
```
|
||
|
||
Expected: 编译失败，seed builder 不存在。
|
||
|
||
- [ ] **Step 3: 实现上一轨迹重采样器**
|
||
|
||
公开只读结果：
|
||
|
||
```csharp
|
||
public sealed class LongitudinalPreviousTrajectorySeed
|
||
{
|
||
public IReadOnlyList<double> PathS { get; }
|
||
public IReadOnlyList<double> ProgressSpeedMetersPerSecond { get; }
|
||
public static LongitudinalPreviousTrajectorySeed Empty { get; }
|
||
}
|
||
```
|
||
|
||
builder 的入口：
|
||
|
||
```csharp
|
||
public LongitudinalPreviousTrajectorySeed Build(EmTrajectory previous,
|
||
LateralPath currentPath, DateTimeOffset newEffectiveAtUtc,
|
||
IReadOnlyList<double> newKnotTimes, int segmentIndex,
|
||
TravelDirection direction)
|
||
```
|
||
|
||
逐 knot 使用绝对时间：
|
||
|
||
```text
|
||
sampleUtc = newEffectiveAtUtc + newKnotTime
|
||
previousTime = sampleUtc - previous.Metadata.EffectiveAtUtc
|
||
```
|
||
|
||
在线性插值上一轨迹世界坐标和 signed speed 后，把世界点投影到 `currentPath.Points[i] -> Points[i+1]` 的折线段，取距离最小且不小于上一投影 PathS 的结果。进度速度使用 `abs(interpolatedSignedSpeed)`。任一输入不兼容时返回 `Empty`，不得抛出影响规划发布的异常。
|
||
|
||
- [ ] **Step 4: 在服务中传递模式和纵向 seed**
|
||
|
||
`EmPlanningService` 在 lateral path 成功后创建 knot 时间并调用 builder：
|
||
|
||
```csharp
|
||
IReadOnlyList<double> knotTimes = LongitudinalCandidate.CreateKnotTimes(
|
||
configuration.Scheduling.TimeHorizonSeconds,
|
||
configuration.Scheduling.OutputTimeStepSeconds);
|
||
LongitudinalPreviousTrajectorySeed previous =
|
||
new LongitudinalPreviousTrajectorySeedBuilder().Build(
|
||
request.PreviousTrajectory, lateral.Path, request.EffectiveAtUtc,
|
||
knotTimes, segment.SegmentIndex, segment.Direction);
|
||
|
||
var longitudinalInput = new LongitudinalPlanningInput(
|
||
lateral.Path, segment.Direction, initialProgressSpeed,
|
||
initialAcceleration, horizon.TerminalType,
|
||
horizon.LongitudinalMode, configuration,
|
||
previous.PathS, previous.ProgressSpeedMetersPerSecond);
|
||
```
|
||
|
||
元数据同时写入 `horizon.LongitudinalMode`。服务切片使用 `horizon.WindowEndReferenceS`。
|
||
|
||
- [ ] **Step 5: 验证服务不再传空 seed**
|
||
|
||
在 `EmPlanningServiceChecks` 的 scripted solver/capture helper 中记录纵向 QP目标项。带有效上一轨迹时断言 previous-S 和 previous-U 权重产生非零对应 Hessian/linear cost；无效上一轨迹时仍然成功且无该项。
|
||
|
||
- [ ] **Step 6: 运行并提交**
|
||
|
||
```powershell
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-model
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-planning-service
|
||
git add -- ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalPreviousTrajectorySeedBuilder.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs
|
||
git commit -m "feat: reuse prior trajectory in longitudinal planning"
|
||
```
|
||
|
||
Expected: `PASS longitudinal-model`、`PASS em-planning-service`。
|
||
|
||
---
|
||
|
||
### Task 8: 服务模式流转、Goal/GearSwitch 与多周期回归
|
||
|
||
**Files:**
|
||
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs`
|
||
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs`
|
||
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/EmFixtureFactory.cs`
|
||
|
||
**Interfaces:**
|
||
- Consumes: Tasks 1–7 的完整规划链路。
|
||
- Produces: 证明 `RollingContinuation -> ApproachStopBoundary -> ExactStopAtBoundary` 的服务级行为。
|
||
|
||
- [ ] **Step 1: 写多周期模式流转测试**
|
||
|
||
创建一条足够长的无障碍直线路径和三组车辆状态：边界在空间窗口外、边界在窗口内但不在时间停车可达范围内、边界在准确停车可达范围内。每次调用 `EmPlanningService.Plan`，断言：
|
||
|
||
```csharp
|
||
Verification.Equal(EmLongitudinalMode.RollingContinuation,
|
||
rolling.Trajectory.Metadata.LongitudinalMode, "cycle 1 rolls");
|
||
Verification.True(rolling.Trajectory.Points[rolling.Trajectory.Points.Count - 1]
|
||
.SignedLongitudinalVelocity != 0d, "cycle 1 has nonzero terminal speed");
|
||
|
||
Verification.Equal(EmLongitudinalMode.ApproachStopBoundary,
|
||
approach.Trajectory.Metadata.LongitudinalMode, "cycle 2 approaches");
|
||
|
||
Verification.Equal(EmLongitudinalMode.ExactStopAtBoundary,
|
||
exact.Trajectory.Metadata.LongitudinalMode, "cycle 3 stops");
|
||
Verification.NearlyEqual(0d, exact.Trajectory.Points[exactAnchor]
|
||
.SignedLongitudinalVelocity, "goal speed is zero");
|
||
Verification.True(exact.Trajectory.Points.Count > exactAnchor + 1,
|
||
"goal anchor is followed by a QP stabilization point");
|
||
Verification.NearlyEqual(exact.Trajectory.Points[exactAnchor].PathS,
|
||
exact.Trajectory.Points[exactAnchor + 1].PathS,
|
||
"goal stabilization keeps the stop position");
|
||
Verification.NearlyEqual(0d, exact.Trajectory.Points[exactAnchor + 1]
|
||
.SignedLongitudinalVelocity, "goal stabilization speed is zero");
|
||
```
|
||
|
||
为 GearSwitch 使用 `CreateGearPairReferencePath()` 重复准确停车检查，并确认发布轨迹不包含下一方向段点。
|
||
|
||
- [ ] **Step 2: 写五米空间窗口/两秒时间范围回归**
|
||
|
||
配置：
|
||
|
||
```csharp
|
||
configuration.Scheduling.DistanceHorizonMeters = 5d;
|
||
configuration.Scheduling.TimeHorizonSeconds = 2d;
|
||
configuration.Scheduling.OutputTimeStepSeconds = 0.1d;
|
||
configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond = 0.2d;
|
||
```
|
||
|
||
断言发布点数为 21，最后一个 PathS 小于 5m，末速非零，并且发布验证通过。
|
||
|
||
- [ ] **Step 3: 运行失败测试并完成最小服务修正**
|
||
|
||
```powershell
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-core-all
|
||
```
|
||
|
||
Expected before fixture updates: service fixture 或 fake solver 仍假定每条轨迹零速停车，至少一个新断言失败。若失败来自 Tasks 1–7 的生产实现，返回所属任务修正并重新执行其测试；本任务只修改测试 fixture，不放宽生产校验。
|
||
|
||
- [ ] **Step 4: 更新旧 rolling service fixture**
|
||
|
||
旧测试通过 `DistanceHorizonMeters=0.003m` 人工制造 rolling terminal；改为使用大于配置停车前视下限的 DistanceHorizon 和更长的参考路径。scripted solver 按 `EmLongitudinalMode` 返回：
|
||
|
||
- rolling/approach：非零末速、无保持尾；
|
||
- exact stop：真实边界锚点及其后的 QP 内部稳定尾 `S/U/A` 准确，允许继续追加外部保持尾。
|
||
|
||
- [ ] **Step 5: 运行并提交**
|
||
|
||
```powershell
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-core-all
|
||
git add -- ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalIntegrationChecks.cs ClumsyPilot/tests/EMPlannerVerificationHost/EmFixtureFactory.cs
|
||
git commit -m "test: cover rolling-to-stop EM planning flow"
|
||
```
|
||
|
||
Expected: `PASS longitudinal-model`、`PASS longitudinal-integration`、`PASS trajectory`、`PASS em-planning-service`。
|
||
|
||
---
|
||
|
||
### Task 9: 观察日志和文档
|
||
|
||
**Files:**
|
||
- Modify: `ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs`
|
||
- Modify: `ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs`
|
||
- Modify: `ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md`
|
||
- Modify: `ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md`
|
||
|
||
**Interfaces:**
|
||
- Consumes: 轨迹元数据中的 `LongitudinalMode` 和 Task 1 停车能力计算。
|
||
- Produces: 启动配置停车能力说明，以及每条成功轨迹的模式和末态摘要。
|
||
|
||
- [ ] **Step 1: 写日志失败测试**
|
||
|
||
在配置诊断断言中增加：
|
||
|
||
```text
|
||
maximumJerkLimitedStopDistance=
|
||
maximumJerkLimitedStopDuration=
|
||
requiredDistanceHorizon=
|
||
```
|
||
|
||
在成功轨迹摘要断言中增加：
|
||
|
||
```text
|
||
longitudinalMode=RollingContinuation
|
||
terminalSpeed=
|
||
terminalAcceleration=
|
||
```
|
||
|
||
服务失败原因断言至少覆盖：
|
||
|
||
```text
|
||
longitudinalMode=
|
||
remainingToBoundary=
|
||
minimumStoppingDistance=
|
||
minimumStoppingDuration=
|
||
maximumStoppedReachableDistance=
|
||
```
|
||
|
||
- [ ] **Step 2: 运行并确认缺少字段**
|
||
|
||
```powershell
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory-observation
|
||
```
|
||
|
||
Expected: 新字段断言失败。
|
||
|
||
- [ ] **Step 3: 扩展一次性配置和每周期摘要**
|
||
|
||
`CreateConfiguration` 使用正向/反向最大速度的较坏情况调用停车模型，并打印：
|
||
|
||
```text
|
||
maximumJerkLimitedStopDistance=0.259m
|
||
maximumJerkLimitedStopDuration=1.800s
|
||
requiredDistanceHorizon=0.299m
|
||
```
|
||
|
||
`CreateTrajectorySummary` 增加：
|
||
|
||
```text
|
||
longitudinalMode=RollingContinuation
|
||
terminalSpeed=0.180m/s
|
||
terminalAcceleration=0.000m/s2
|
||
```
|
||
|
||
这些配置仍只在 session 启动时打印一次，不加入 UI 每帧重绘块。
|
||
|
||
- [ ] **Step 4: 更新 README**
|
||
|
||
在 EMPlanner README 明确：
|
||
|
||
```text
|
||
DistanceHorizonMeters controls the LS reference window.
|
||
TimeHorizonSeconds controls the ST output duration.
|
||
Only Goal/GearSwitchApproach may require an exact zero-speed terminal.
|
||
RollingContinuation and ApproachStopBoundary may publish nonzero terminal speed.
|
||
```
|
||
|
||
在 MovementTest README 明确仍为 `OBSERVE_ONLY`，滚动轨迹不会下发底盘。
|
||
|
||
- [ ] **Step 5: 运行并提交**
|
||
|
||
```powershell
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory-observation
|
||
git add -- ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationDiagnostics.cs ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/README.md
|
||
git commit -m "docs: explain rolling longitudinal planning diagnostics"
|
||
```
|
||
|
||
Expected: `PASS trajectory-observation`。
|
||
|
||
---
|
||
|
||
### Task 10: 全量回归和目标机 OSQP 验证
|
||
|
||
**Files:**
|
||
- No file changes planned; this task is verification-only。
|
||
- Do not modify: unrelated dirty PathSmoothing、CoarsePath、Map or user configuration files。
|
||
|
||
**Interfaces:**
|
||
- Consumes: all prior tasks。
|
||
- Produces: verified implementation ready for observation-only target-machine testing。
|
||
|
||
- [ ] **Step 1: 运行核心回归**
|
||
|
||
```powershell
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-core-all
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- coordinator
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- executor
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- trajectory-observation
|
||
```
|
||
|
||
Expected: 每条命令分别输出它在 `Program.cs` 中定义的 `PASS` 行；不出现新的编译错误或验证失败。
|
||
|
||
- [ ] **Step 2: 构建主工程**
|
||
|
||
```powershell
|
||
dotnet build ClumsyPilot/ClumsyPilot.csproj -p:ExcludeLegacyAutoAvoidance=true
|
||
```
|
||
|
||
Expected: Build succeeded。已有 obsolete warning 可以保留，但不得新增本计划相关 warning。
|
||
|
||
- [ ] **Step 3: 在装有正确 OSQP 原生依赖的目标机运行真实求解探针**
|
||
|
||
```powershell
|
||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- longitudinal-real-osqp-probe
|
||
```
|
||
|
||
Expected: `PASS longitudinal-real-osqp`。若原生 DLL 无法加载，只记录为目标机依赖问题，不通过修改纵向逻辑或放宽残差阈值绕过。
|
||
|
||
- [ ] **Step 4: 运行 MovementTest observation-only 实测**
|
||
|
||
使用当前手工参数启动 `TrajectoryObservationMovementTest`，确认：
|
||
|
||
1. 启动时只打印一次规划配置和最大 jerk 受限停车能力；
|
||
2. 远离 Goal 时发布 `RollingContinuation`，末速非零且无 hold；
|
||
3. 接近 Goal 时出现 `ApproachStopBoundary`；
|
||
4. 最终出现 `ExactStopAtBoundary`，终端 `U=0,A=0`；
|
||
5. 不再出现 point 21 `JerkLimitExceeded`；
|
||
6. 终端持续打印 `OBSERVE_ONLY: no chassis command is sent.`。
|
||
|
||
- [ ] **Step 5: 检查只包含计划内改动**
|
||
|
||
```powershell
|
||
git status --short
|
||
git log --oneline -12
|
||
```
|
||
|
||
逐个核对 Tasks 1–9 的提交；不得暂存或提交用户现有 PathSmoothing、CoarsePath、Map 和 MovementTest 手工配置改动。
|
||
|
||
- [ ] **Step 6: 处理最终回归结果**
|
||
|
||
若 Step 1–4 失败，返回引入该行为的 Task 1–9，在该任务列出的文件范围和测试命令内修正并追加对应任务提交；不得用一个无法确定所有权的“最终修复”提交混合多个模块。全部通过时不创建空提交。
|