feat: publish one-shot EM trajectories

2026-08-04 11:55:20 +08:00
parent 7410654e68
commit 019b89645a
5 changed files with 730 additions and 1 deletions
@@ -0,0 +1,209 @@
+using System;
+using System.Collections.Generic;
+using System.Threading;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Runs one deterministic EM LS/ST planning pipeline and publishes only independently validated trajectories.</summary>
+public sealed class EmPlanningService : IEmPlanningService
+{
+    private readonly IQpSolver qpSolver;
+    private readonly IEmPlannerDebugSink defaultDebugSink;
+
+    public EmPlanningService(IQpSolver qpSolver, IEmPlannerDebugSink defaultDebugSink = null)
+    {
+        this.qpSolver = qpSolver ?? throw new ArgumentNullException(nameof(qpSolver));
+        this.defaultDebugSink = defaultDebugSink;
+    }
+
+    public EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken)
+    {
+        if (cancellationToken.IsCancellationRequested)
+            return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled before request validation.");
+
+        EmPlanningRequestValidationResult requestValidation = EmPlanningRequestValidator.Validate(request);
+        if (!requestValidation.IsValid)
+            return Failure(requestValidation.Status, request, requestValidation.FailureReason);
+        EmPlannerConfiguration configuration = requestValidation.Snapshot.Configuration;
+        EmitDebug(request, "request/config validation succeeded");
+
+        try
+        {
+            IReadOnlyList<DirectionSegmentView> segments = ReferencePathSegmenter.Create(request.ReferencePath);
+            if (request.SegmentIndex < 0 || request.SegmentIndex >= segments.Count)
+                return Failure(EmPlanningStatus.InvalidReferencePath, request, "The requested direction segment is unavailable.");
+            DirectionSegmentView segment = segments[request.SegmentIndex];
+            if (!HasCompatibleStateDirection(request.VehicleState, segment.Direction,
+                    configuration.Longitudinal.StopSpeedToleranceMetersPerSecond))
+            {
+                return Failure(EmPlanningStatus.StateDirectionMismatch, request,
+                    "Vehicle signed speed contradicts the selected direction segment.");
+            }
+            EmitDebug(request, "direction-segment selection succeeded");
+
+            var projector = new FrenetProjector();
+            if (!projector.TryProject(request.VehicleState.Pose, segment, 0d, segment.LengthMeters,
+                    configuration.Frenet.MaximumProjectionDistanceMeters, 0d, out FrenetProjection startProjection))
+            {
+                return Failure(EmPlanningStatus.ProjectionFailed, request,
+                    "Vehicle pose could not be projected inside the selected direction segment.");
+            }
+            EmitDebug(request, "bounded ego projection succeeded");
+
+            double initialProgressSpeed = Math.Abs(request.VehicleState.SignedLongitudinalSpeedMetersPerSecond);
+            double initialAcceleration = request.VehicleState.LongitudinalAccelerationMetersPerSecondSquared ?? 0d;
+            var horizonSelector = new PlanningHorizonSelector();
+            EmPlanningStatus horizonStatus = horizonSelector.Select(segment, startProjection.ReferenceS, initialProgressSpeed,
+                initialAcceleration, configuration, out PlanningHorizonSelection horizon, out string horizonReason);
+            if (horizonStatus != EmPlanningStatus.Success)
+                return Failure(horizonStatus, request, horizonReason);
+            ReferenceHorizonSlice slice = ReferenceHorizonSlicer.Slice(segment, horizon.TerminalReferenceS);
+            EmitDebug(request, "exact horizon and terminal selection succeeded");
+
+            IReadOnlyList<FrenetProjection> previousSeed = ProjectPreviousTrajectorySeed(request.PreviousTrajectory, segment,
+                startProjection.ReferenceS, horizon.TerminalReferenceS, configuration.Frenet.MaximumProjectionDistanceMeters);
+            var corridorSeed = new List<FrenetProjection>(previousSeed.Count + 1) { startProjection };
+            for (int index = 0; index < previousSeed.Count; index++) corridorSeed.Add(previousSeed[index]);
+            EmitDebug(request, "previous-trajectory seed projection completed");
+
+            var corridorBuilder = new StaticCorridorBuilder();
+            if (!corridorBuilder.TryBuild(segment, startProjection.ReferenceS, slice.TerminalBoundary.SegmentLocalS, corridorSeed,
+                    request.Map, request.Vehicle, configuration.Corridor, out StaticCorridor corridor, out string corridorReason))
+            {
+                return Failure(EmPlanningStatus.CorridorInfeasible, request, corridorReason);
+            }
+            EmitDebug(request, "static connected corridor succeeded");
+
+            var lateralInput = new LateralPlanningInput(segment, corridor, startProjection, horizon.TerminalType,
+                request.Vehicle, configuration, previousSeed);
+            LateralPlanningResult lateral = new LateralPlanner(qpSolver).Plan(lateralInput, cancellationToken);
+            if (!IsSuccess(lateral.Status))
+                return Failure(lateral.Status, request, lateral.FailureReason);
+            EmitDebug(request, "LS optimization and validation succeeded");
+
+            var longitudinalInput = new LongitudinalPlanningInput(lateral.Path, segment.Direction, initialProgressSpeed,
+                initialAcceleration, horizon.TerminalType, configuration, Array.Empty<double>(), Array.Empty<double>());
+            EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(longitudinalInput, out _, out string envelopeReason);
+            if (envelopeStatus != EmPlanningStatus.Success)
+                return Failure(envelopeStatus, request, envelopeReason);
+            EmitDebug(request, "PathS speed envelope succeeded");
+
+            LongitudinalPlanningResult longitudinal = new LongitudinalPlanner(qpSolver).Plan(longitudinalInput, cancellationToken);
+            if (!IsSuccess(longitudinal.Status))
+                return Failure(longitudinal.Status, request, longitudinal.FailureReason);
+            EmitDebug(request, "ST optimization and validation succeeded");
+
+            var metadata = new EmTrajectoryMetadata(request.OutputTrajectoryId, request.RequestedAtUtc, request.EffectiveAtUtc,
+                request.Map.SnapshotId, request.ReferencePathId, request.VehicleState.SequenceId, request.PreviousTrajectoryId,
+                segment.SegmentIndex, segment.Direction, horizon.TerminalType);
+            EmTrajectory trajectory = new EmTrajectoryAssembler(configuration).Assemble(lateral.Path, longitudinal, metadata);
+            EmitDebug(request, "trajectory assembly succeeded");
+
+            EmTrajectoryValidationResult publication = new EmTrajectoryValidator().Validate(trajectory, request.Map,
+                request.Vehicle, configuration, segment.SegmentIndex, longitudinalInput.TerminalPathS,
+                slice.TerminalBoundary.BoundaryType);
+            if (!publication.IsValid)
+            {
+                return Failure(EmPlanningStatus.ValidationFailed, request,
+                    publication.Failure + " at point " + publication.PointIndex + ": " + publication.Message);
+            }
+            EmitDebug(request, "world-space publication validation succeeded");
+
+            EmPlanningStatus finalStatus = lateral.Status == EmPlanningStatus.SuccessWithFallback ||
+                longitudinal.Status == EmPlanningStatus.SuccessWithFallback
+                ? EmPlanningStatus.SuccessWithFallback
+                : EmPlanningStatus.Success;
+            return new EmPlanningResult(finalStatus, trajectory,
+                DiagnosticsPrefix(request) + ";terminal=" + horizon.TerminalType + ";publication=validated");
+        }
+        catch (OperationCanceledException)
+        {
+            return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled.");
+        }
+        catch (ArgumentException exception)
+        {
+            return Failure(EmPlanningStatus.Failed, request, exception.Message);
+        }
+        catch (InvalidOperationException exception)
+        {
+            return Failure(EmPlanningStatus.Failed, request, exception.Message);
+        }
+    }
+
+    private void EmitDebug(EmPlanningRequest request, string message)
+    {
+        if (defaultDebugSink == null || request == null || request.Configuration == null || request.Configuration.Solver == null ||
+            !request.Configuration.Solver.NativeVerbose)
+        {
+            return;
+        }
+        try
+        {
+            defaultDebugSink.Write(message ?? string.Empty);
+        }
+        catch
+        {
+            // Debug output is deliberately isolated from pure planning results.
+        }
+    }
+
+    private static IReadOnlyList<FrenetProjection> ProjectPreviousTrajectorySeed(EmTrajectory previousTrajectory,
+        DirectionSegmentView segment, double minimumReferenceS, double maximumReferenceS, double maximumDistanceMeters)
+    {
+        var projected = new List<FrenetProjection>();
+        if (previousTrajectory == null || previousTrajectory.Metadata.SegmentIndex != segment.SegmentIndex ||
+            previousTrajectory.Metadata.Direction != segment.Direction)
+        {
+            return projected;
+        }
+
+        var projector = new FrenetProjector();
+        double seedReferenceS = minimumReferenceS;
+        for (int index = 0; index < previousTrajectory.Points.Count; index++)
+        {
+            EmTrajectoryPoint point = previousTrajectory.Points[index];
+            if (point == null || point.TimeFromStart <= 0d || point.Direction != segment.Direction)
+                continue;
+            if (projector.TryProject(new Pose2D(point.X, point.Y, point.Yaw), segment, minimumReferenceS,
+                    maximumReferenceS, maximumDistanceMeters, seedReferenceS, out FrenetProjection projection))
+            {
+                projected.Add(projection);
+                seedReferenceS = projection.ReferenceS;
+            }
+        }
+        return projected;
+    }
+
+    private static bool HasCompatibleStateDirection(VehicleMotionState state, TravelDirection direction, double stopTolerance)
+    {
+        if (state == null || double.IsNaN(stopTolerance) || double.IsInfinity(stopTolerance) || stopTolerance < 0d)
+            return false;
+        if (Math.Abs(state.SignedLongitudinalSpeedMetersPerSecond) <= stopTolerance)
+            return true;
+        return direction == TravelDirection.Forward
+            ? state.SignedLongitudinalSpeedMetersPerSecond > 0d
+            : state.SignedLongitudinalSpeedMetersPerSecond < 0d;
+    }
+
+    private static bool IsSuccess(EmPlanningStatus status)
+    {
+        return status == EmPlanningStatus.Success || status == EmPlanningStatus.SuccessWithFallback;
+    }
+
+    private static EmPlanningResult Failure(EmPlanningStatus status, EmPlanningRequest request, string reason)
+    {
+        return new EmPlanningResult(status, null, DiagnosticsPrefix(request) + ";reason=" + (reason ?? string.Empty));
+    }
+
+    private static string DiagnosticsPrefix(EmPlanningRequest request)
+    {
+        if (request == null)
+            return "map=;reference=;state=;previous=;segment=";
+        return "map=" + (request.Map == null ? string.Empty : request.Map.SnapshotId.ToString()) +
+            ";reference=" + (request.ReferencePathId ?? string.Empty) +
+            ";state=" + (request.VehicleState == null ? string.Empty : request.VehicleState.SequenceId.ToString()) +
+            ";previous=" + (request.PreviousTrajectoryId ?? string.Empty) +
+            ";segment=" + request.SegmentIndex;
+    }
+}
@@ -0,0 +1,9 @@
+using System.Threading;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Pure, one-shot EM planning boundary with no scheduler, UI, hardware, or clock dependency.</summary>
+public interface IEmPlanningService
+{
+    EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken);
+}
@@ -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
+```