feat: publish one-shot EM trajectories
This commit is contained in:
梁薄云
@@ -214,3 +214,132 @@ dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerifi
|
||||
该门禁依次验证 LS 模型、脚本化 SQP 状态机，以及在干净复制 plugin bundle 中运行的真实 OSQP
|
||||
固定场景：前进/倒车直线、缓弯、静态障碍收窄的种子连通走廊、换向终端和滚动终端。每个真实
|
||||
场景运行两次，状态、点数和全部数值输出必须在 `1e-10` 内一致。
|
||||
|
||||
## ST、完整轨迹与单次服务
|
||||
|
||||
ST 只消费 LS 已复核的实际 `PathS`，而不把 `ReferenceS` 当作行驶距离。它在固定时间 knot 上
|
||||
求解非负进度速度 `u`、加速度和 jerk，并在所有终端硬约束 `PathS=terminalPathS`、`u=0`。
|
||||
速度包络取方向限速、横向加速度、曲率率和停车包络中的最小值。成功的轨迹始终包含精确零速终端，
|
||||
随后按 `0.05 s` 间隔提供 `0.20 s` 的同姿态、零速度 hold tail。
|
||||
|
||||
公开门面仅提供单次、同步且可取消的调用：
|
||||
|
||||
```csharp
|
||||
EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken);
|
||||
```
|
||||
|
||||
`EmPlanningService` 不负责周期调度、版本淘汰、轨迹执行、换向状态机、控制适配、UI 或硬件读取。
|
||||
它只使用请求提供的 `RequestedAtUtc` / `EffectiveAtUtc`，绝不读取系统时钟或当前工作目录。处理顺序固定为：
|
||||
|
||||
```text
|
||||
request/config validation
|
||||
-> direction-segment selection
|
||||
-> bounded ego projection
|
||||
-> PlanningHorizonSelector exact terminal
|
||||
-> previous-trajectory seed projection
|
||||
-> static connected corridor
|
||||
-> LS optimization and validation
|
||||
-> PathS speed envelope
|
||||
-> ST optimization and validation
|
||||
-> immutable trajectory assembly
|
||||
-> world-space publication validation
|
||||
-> immutable result publication
|
||||
```
|
||||
|
||||
### 请求快照
|
||||
|
||||
`EmPlanningRequest` 的构造参数依次为：已发布的 `PathSmoothingResult`、同版本的
|
||||
`PlanningGridMap`、`VehicleParameters`、不可变 `VehicleMotionState`、`EmPlannerConfiguration`、
|
||||
当前 `SegmentIndex`、可选 `PreviousTrajectory`、`RequestedAtUtc`、`EffectiveAtUtc`、输出轨迹 ID、
|
||||
参考路径 ID、上一轨迹 ID，以及 `EmMotionModel.NonholonomicForwardReverse`。状态快照中的正带符号
|
||||
速度表示前进，负值表示倒车；绝对值不大于 `StopSpeedToleranceMetersPerSecond` 时按停车处理。
|
||||
|
||||
服务会复制配置和各规划输入，不修改请求所属对象或列表。结果诊断（`FailureReason`）总是以以下稳定
|
||||
标识开始，方便调用方审计版本绑定：
|
||||
|
||||
```text
|
||||
map=<MapSnapshotId>;reference=<ReferencePathId>;state=<VehicleState.SequenceId>;
|
||||
previous=<PreviousTrajectoryId>;segment=<SegmentIndex>
|
||||
```
|
||||
|
||||
### 结果、字段和单位
|
||||
|
||||
`EmPlanningResult` 只有 `Success` 或 `SuccessWithFallback` 时才携带不可变 `EmTrajectory`；所有失败状态
|
||||
都携带空轨迹。`EmTrajectory.Metadata` 包含轨迹 ID、生成/生效时间、地图 ID、参考路径 ID、状态序列、
|
||||
上一轨迹 ID、方向段、方向和终端类型。每个公开 `EmTrajectoryPoint` 字段如下：
|
||||
|
||||
| 字段 | 单位 / 符号 |
|
||||
| --- | --- |
|
||||
| `X`, `Y` | 世界坐标 m |
|
||||
| `Yaw` | 车辆车头世界航向 rad，归一化到 `[-PI, PI)` |
|
||||
| `SignedLongitudinalVelocity` | 车体纵向 m/s；前进为正，倒车为负；权威速度字段 |
|
||||
| `Speed` | `abs(SignedLongitudinalVelocity)`，m/s，非负 |
|
||||
| `VelocityX`, `VelocityY` | 世界速度 m/s；分别等于 `signedV*cos(Yaw)`、`signedV*sin(Yaw)` |
|
||||
| `YawRate` | rad/s，逆时针为正；等于 `signedV*VehicleCurvature` |
|
||||
| `TimeFromStart` | 从本条轨迹生效时刻起的 s，严格递增 |
|
||||
| `VehicleCurvature` | 车辆曲率 `1/m`；倒车时已按车头 yaw 符号转换 |
|
||||
| `SegmentIndex`, `SegmentLocalS`, `PathS` | 当前方向段标识与局部实际进度 m；`PathS` 不递减 |
|
||||
| `Direction`, `BoundaryType` | `Forward` / `Reverse` 与 `None`、`RollingSafetyStop`、`GearSwitchApproach` 或 `Goal` |
|
||||
|
||||
冗余速度字段不可独立赋值；组装器从权威 `signedV` 和 `VehicleCurvature` 派生它们，发布前
|
||||
`EmTrajectoryValidator` 再独立复算。验证器还会重算有限差分加速度、jerk、曲率率，逐点调用完整
|
||||
车体姿态检查，并以最大 `0.025 m` 中心步长检查每个相邻点的扫掠运动。
|
||||
|
||||
终端类型固定为：
|
||||
|
||||
| `EmTerminalType` | 含义 |
|
||||
| --- | --- |
|
||||
| `RollingSafetyStop` | 当前规划窗口未到分段边界时的安全停车终端 |
|
||||
| `GearSwitch` | 当前方向段末端的精确停车；执行层随后拥有换向状态机 |
|
||||
| `Goal` | 最后方向段末端的精确停车 |
|
||||
|
||||
状态为 `Success`、`SuccessWithFallback`、`InvalidInput`、`UnsupportedMotionMode`、`StaleVehicleState`、
|
||||
`StateDirectionMismatch`、`InvalidReferencePath`、`ProjectionFailed`、`CorridorInfeasible`、
|
||||
`LateralInfeasible`、`LongitudinalInfeasible`、`StoppingDistanceInsufficient`、`SolverUnavailable`、
|
||||
`SolverTimedOut`、`Cancelled`、`ValidationFailed`、`Superseded` 和 `Failed`。除前两项外，全部状态
|
||||
均发布空轨迹和确定性诊断。
|
||||
|
||||
### 最小调用示例
|
||||
|
||||
调用方先在规划边界外获取地图、平滑路径和车辆状态快照；下面的对象均为该步骤已经准备好的不可变输入：
|
||||
|
||||
```csharp
|
||||
var service = new EmPlanningService(qpSolver);
|
||||
var forwardRequest = new EmPlanningRequest(
|
||||
publishedSmoothingResult, planningMap, vehicle, forwardState, configuration,
|
||||
segmentIndex: 0, previousTrajectory: null,
|
||||
requestedAtUtc: capturedRequestTime, effectiveAtUtc: effectiveTime,
|
||||
outputTrajectoryId: "traj-100", referencePathId: "path-17", previousTrajectoryId: "",
|
||||
motionModel: EmMotionModel.NonholonomicForwardReverse);
|
||||
|
||||
EmPlanningResult forward = service.Plan(forwardRequest, cancellationToken);
|
||||
```
|
||||
|
||||
倒车不需要额外翻转横向坐标或世界速度；选择倒车方向段并把状态带符号速度设为负即可：
|
||||
|
||||
```csharp
|
||||
var reverseState = new VehicleMotionState(reversePose, -0.05d, null, capturedRequestTime, sequenceId: 44);
|
||||
var reverseRequest = new EmPlanningRequest(
|
||||
publishedSmoothingResult, planningMap, vehicle, reverseState, configuration,
|
||||
segmentIndex: 1, previousTrajectory: forward.Trajectory,
|
||||
requestedAtUtc: capturedRequestTime, effectiveAtUtc: effectiveTime,
|
||||
outputTrajectoryId: "traj-101", referencePathId: "path-17", previousTrajectoryId: "traj-100",
|
||||
motionModel: EmMotionModel.NonholonomicForwardReverse);
|
||||
|
||||
EmPlanningResult reverse = service.Plan(reverseRequest, cancellationToken);
|
||||
```
|
||||
|
||||
在仓库根目录运行完整单次服务门禁：
|
||||
|
||||
```powershell
|
||||
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-core-all
|
||||
```
|
||||
|
||||
成功输出依次为：
|
||||
|
||||
```text
|
||||
PASS longitudinal-model
|
||||
PASS longitudinal-integration
|
||||
PASS trajectory
|
||||
PASS em-planning-service
|
||||
```
|
||||
|
||||
Reference in New Issue
Block a user