feat: publish one-shot EM trajectories

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs

@@ -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;
+    }
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/IEmPlanningService.cs

@@ -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);
+}

ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md

@@ -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
+```

ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs

@@ -0,0 +1,367 @@
+using System;
+using System.Collections.Generic;
+using System.Reflection;
+using System.Threading;
+using EMPlannerVerificationHost;
+using MultiWheelC.TrajectoryPlanning.CoarsePath;
+using MultiWheelC.TrajectoryPlanning.Mapping;
+using MultiWheelC.TrajectoryPlanning.PathSmoothing;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+internal static class EmPlanningServiceChecks
+{
+    public static void Run()
+    {
+        VerifiesForwardReverseAndBoundarySuccessesAreDeterministic();
+        VerifiesRequestAndStateFailuresPublishNoTrajectory();
+        VerifiesProjectionCorridorAndOptimizationFailuresPublishNoTrajectory();
+        VerifiesTimeoutFallbackAndCancellationSemantics();
+        VerifiesPublicationFailureAndDebugIsolation();
+    }
+
+    private static void VerifiesForwardReverseAndBoundarySuccessesAreDeterministic()
+    {
+        EmPlanningRequest forwardRequest = CreateRequest(TravelDirection.Forward, 0d, false, false);
+        var forwardService = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success));
+        EmPlanningResult firstForward = forwardService.Plan(forwardRequest, CancellationToken.None);
+        EmPlanningResult secondForward = forwardService.Plan(forwardRequest, CancellationToken.None);
+        VerifySuccess(firstForward, forwardRequest, EmTerminalType.Goal, "forward");
+        VerifySuccess(secondForward, forwardRequest, EmTerminalType.Goal, "forward repeat");
+        VerifySameTrajectory(firstForward, secondForward, "forward deterministic result");
+        Verification.Equal(2, forwardRequest.ReferencePath.Path.Count, "request-owned reference list remains unchanged");
+
+        EmPlanningRequest reverseRequest = CreateRequest(TravelDirection.Reverse, 0d, false, false);
+        EmPlanningResult reverse = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(reverseRequest,
+            CancellationToken.None);
+        VerifySuccess(reverse, reverseRequest, EmTerminalType.Goal, "reverse");
+        Verification.True(reverse.Trajectory.Points[1].SignedLongitudinalVelocity < 0d,
+            "reverse service publishes negative signed velocity");
+
+        EmPlanningRequest gearRequest = CreateRequest(TravelDirection.Forward, 0d, true, false);
+        EmPlanningResult gear = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(gearRequest,
+            CancellationToken.None);
+        VerifySuccess(gear, gearRequest, EmTerminalType.GearSwitch, "gear switch");
+
+        EmPlanningRequest rollingRequest = CreateRequest(TravelDirection.Forward, 0d, false, true);
+        EmPlanningResult rolling = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(rollingRequest,
+            CancellationToken.None);
+        VerifySuccess(rolling, rollingRequest, EmTerminalType.RollingSafetyStop, "rolling stop");
+    }
+
+    private static void VerifiesRequestAndStateFailuresPublishNoTrajectory()
+    {
+        EmPlanningRequest invalidSmoothing = CreateRequest(TravelDirection.Forward, 0d, false, false,
+            PathSmoothingResult.Failure(PathSmoothingStatus.Failed, new PathSmoothingDiagnostics()));
+        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(invalidSmoothing,
+            CancellationToken.None), EmPlanningStatus.InvalidReferencePath, "invalid smoothing");
+
+        EmPlanningRequest stale = CreateRequest(TravelDirection.Forward, 0d, false, false);
+        stale = ReplaceState(stale, new VehicleMotionState(new Pose2D(0d, 0d, 0d), 0d, null,
+            stale.RequestedAtUtc.AddSeconds(-1d), stale.VehicleState.SequenceId));
+        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(stale,
+            CancellationToken.None), EmPlanningStatus.StaleVehicleState, "stale state");
+
+        EmPlanningRequest directionMismatch = CreateRequest(TravelDirection.Reverse, 0.02d, false, false);
+        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(directionMismatch,
+            CancellationToken.None), EmPlanningStatus.StateDirectionMismatch, "state direction mismatch");
+    }
+
+    private static void VerifiesProjectionCorridorAndOptimizationFailuresPublishNoTrajectory()
+    {
+        EmPlanningRequest projectionFailure = CreateRequest(TravelDirection.Forward, 0d, false, false);
+        projectionFailure = ReplaceState(projectionFailure, new VehicleMotionState(new Pose2D(1d, 0d, 0d), 0d, null,
+            projectionFailure.RequestedAtUtc, projectionFailure.VehicleState.SequenceId));
+        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(projectionFailure,
+            CancellationToken.None), EmPlanningStatus.ProjectionFailed, "bounded projection failure");
+
+        EmPlanningRequest corridorFailure = CreateRequest(TravelDirection.Forward, 0d, false, false,
+            referencePath: null, map: CreateMap(true));
+        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(corridorFailure,
+            CancellationToken.None), EmPlanningStatus.CorridorInfeasible, "corridor infeasible");
+
+        EmPlanningRequest stoppingFailure = CreateRequest(TravelDirection.Forward, 0.20d, false, false);
+        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(stoppingFailure,
+            CancellationToken.None), EmPlanningStatus.StoppingDistanceInsufficient, "stopping distance insufficient");
+
+        EmPlanningRequest regular = CreateRequest(TravelDirection.Forward, 0d, false, false);
+        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.LateralInfeasible)).Plan(regular,
+            CancellationToken.None), EmPlanningStatus.LateralInfeasible, "lateral infeasible");
+        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.LongitudinalInfeasible)).Plan(regular,
+            CancellationToken.None), EmPlanningStatus.LongitudinalInfeasible, "longitudinal infeasible");
+        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.SolverUnavailable)).Plan(regular,
+            CancellationToken.None), EmPlanningStatus.SolverUnavailable, "solver unavailable");
+    }
+
+    private static void VerifiesTimeoutFallbackAndCancellationSemantics()
+    {
+        EmPlanningRequest request = CreateRequest(TravelDirection.Forward, 0d, false, false);
+        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.TimeoutWithoutFallback)).Plan(request,
+            CancellationToken.None), EmPlanningStatus.SolverTimedOut, "timeout without fallback");
+
+        EmPlanningResult fallback = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.TimeoutWithFallback)).Plan(
+            request, CancellationToken.None);
+        Verification.Equal(EmPlanningStatus.SuccessWithFallback, fallback.Status, "timeout uses only strict fallback");
+        Verification.True(fallback.Trajectory != null && fallback.Trajectory.Points.Count > 0,
+            "timeout fallback publishes a complete trajectory");
+
+        using (var cancellation = new CancellationTokenSource())
+        {
+            cancellation.Cancel();
+            VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success)).Plan(request,
+                cancellation.Token), EmPlanningStatus.Cancelled, "cancellation");
+        }
+    }
+
+    private static void VerifiesPublicationFailureAndDebugIsolation()
+    {
+        EmPlanningRequest validationFailure = CreateRequest(TravelDirection.Forward, 0d, false, false);
+        VerifyFailure(new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.PublicationValidationFailure,
+            validationFailure.Map)).Plan(validationFailure, CancellationToken.None), EmPlanningStatus.ValidationFailed,
+            "publication validation failure");
+
+        EmPlanningRequest debugRequest = CreateRequest(TravelDirection.Forward, 0d, false, false);
+        debugRequest.Configuration.Solver.NativeVerbose = true;
+        EmPlanningResult debugIsolated = new EmPlanningService(new ScriptedPipelineSolver(PipelineSolverMode.Success),
+            new ThrowingDebugSink()).Plan(debugRequest, CancellationToken.None);
+        VerifySuccess(debugIsolated, debugRequest, EmTerminalType.Goal, "debug-sink isolation");
+    }
+
+    private static void VerifySuccess(EmPlanningResult result, EmPlanningRequest request, EmTerminalType terminalType,
+        string name)
+    {
+        Verification.True(result.Status == EmPlanningStatus.Success || result.Status == EmPlanningStatus.SuccessWithFallback,
+            name + " successful status: " + result.FailureReason);
+        Verification.True(result.Trajectory != null && result.Trajectory.Metadata.TerminalType == terminalType,
+            name + " terminal type");
+        string identifiers = "map=" + request.Map.SnapshotId + ";reference=" + request.ReferencePathId + ";state=" +
+            request.VehicleState.SequenceId + ";previous=" + request.PreviousTrajectoryId + ";segment=" + request.SegmentIndex;
+        Verification.True(result.FailureReason.IndexOf(identifiers, StringComparison.Ordinal) >= 0,
+            name + " preserves request identifiers in deterministic diagnostics");
+    }
+
+    private static void VerifyFailure(EmPlanningResult result, EmPlanningStatus expected, string name)
+    {
+        Verification.Equal(expected, result.Status, name + " status: " + result.FailureReason);
+        Verification.True(result.Trajectory == null, name + " publishes no partial trajectory");
+    }
+
+    private static void VerifySameTrajectory(EmPlanningResult left, EmPlanningResult right, string name)
+    {
+        Verification.Equal(left.Status, right.Status, name + " status");
+        Verification.Equal(left.Trajectory.Points.Count, right.Trajectory.Points.Count, name + " point count");
+        for (int index = 0; index < left.Trajectory.Points.Count; index++)
+        {
+            EmTrajectoryPoint first = left.Trajectory.Points[index];
+            EmTrajectoryPoint second = right.Trajectory.Points[index];
+            Verification.NearlyEqual(first.X, second.X, name + " X " + index);
+            Verification.NearlyEqual(first.Y, second.Y, name + " Y " + index);
+            Verification.NearlyEqual(first.TimeFromStart, second.TimeFromStart, name + " time " + index);
+            Verification.NearlyEqual(first.SignedLongitudinalVelocity, second.SignedLongitudinalVelocity,
+                name + " signed speed " + index);
+        }
+    }
+
+    private static EmPlanningRequest CreateRequest(TravelDirection direction, double signedSpeed, bool endsAtGearSwitch,
+        bool rolling, PathSmoothingResult? referencePath = null, PlanningGridMap? map = null)
+    {
+        DateTimeOffset requestedAtUtc = DateTimeOffset.UnixEpoch.AddSeconds(10d);
+        EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
+        configuration.Solver.MaximumOuterIterations = 2;
+        configuration.Scheduling.SolverTimeoutSeconds = 1d;
+        if (rolling)
+            configuration.Scheduling.DistanceHorizonMeters = 0.003d;
+        return new EmPlanningRequest(referencePath ?? CreateReferencePath(direction, endsAtGearSwitch), map ?? CreateMap(false),
+            new VehicleParameters
+            {
+                LengthMeters = 0.10d,
+                WidthMeters = 0.10d,
+                SafetyMarginMeters = 0d,
+                MaximumCurvaturePerMeter = 1d,
+            },
+            new VehicleMotionState(new Pose2D(0d, 0d, 0d), signedSpeed, 0d, requestedAtUtc, 7L), configuration, 0,
+            null, requestedAtUtc, requestedAtUtc, "output-trajectory", "reference-42", "prior-42",
+            EmMotionModel.NonholonomicForwardReverse);
+    }
+
+    private static EmPlanningRequest ReplaceState(EmPlanningRequest source, VehicleMotionState state)
+    {
+        return new EmPlanningRequest(source.ReferencePath, source.Map, source.Vehicle, state, source.Configuration,
+            source.SegmentIndex, source.PreviousTrajectory, source.RequestedAtUtc, source.EffectiveAtUtc,
+            source.OutputTrajectoryId, source.ReferencePathId, source.PreviousTrajectoryId, source.MotionModel);
+    }
+
+    private static PathSmoothingResult CreateReferencePath(TravelDirection direction, bool endsAtGearSwitch)
+    {
+        double endX = direction == TravelDirection.Forward ? 0.0055d : -0.0055d;
+        var points = new List<SmoothedPathPoint>
+        {
+            new SmoothedPathPoint(0d, 0d, 0d, 0d, 0d, direction, 0d, 0d, 0d, 1d, false,
+                SmoothedPathPointSource.Anchor),
+            new SmoothedPathPoint(endX, 0d, 0d, 0d, 0.0055d, direction, 0d, 0d, 0d, 1d, endsAtGearSwitch,
+                endsAtGearSwitch ? SmoothedPathPointSource.GearSwitch : SmoothedPathPointSource.Anchor),
+        };
+        var segments = new List<SmoothedPathSegment>
+        {
+            new SmoothedPathSegment(0, direction, 0, 1, false, endsAtGearSwitch),
+        };
+        var metrics = new PathQualityMetrics(true, 0.0055d, 0d, 0d, 0d, 0d, 1d, 0d, 0d, 0d, 0d, 0d);
+        return PathSmoothingResult.PublishLocalG2(PathSmoothingStatus.Complete, points, segments,
+            new PathSmoothingDiagnostics(metrics, TimeSpan.Zero), new List<PathSmoothingRegionReport>());
+    }
+
+    private static PlanningGridMap CreateMap(bool blockStart)
+    {
+        IMapObstacleSource[] sources = blockStart
+            ? new IMapObstacleSource[] { new ManualObstacleSource("service-obstacle", 1L, true,
+                new IMapObstacle[] { new AxisAlignedRectangleObstacle(-20f, 20f, -20f, 20f) }) }
+            : Array.Empty<IMapObstacleSource>();
+        PlanningMapBuildResult result = new PlanningMapFactory().Create(new PlanningMapRequest
+        {
+            Bounds = new MapBoundsMm(-1000f, 3000f, -1000f, 1000f),
+            ResolutionMm = 20f,
+            ObstacleSources = sources,
+            AllowExplicitEmptyMap = !blockStart,
+        });
+        Verification.True(result.Succeeded && result.Map != null && result.Map.PlanningReady,
+            "service map builds: " + result.FailureReason);
+        return result.Map!;
+    }
+
+    private enum PipelineSolverMode
+    {
+        Success,
+        LateralInfeasible,
+        LongitudinalInfeasible,
+        SolverUnavailable,
+        TimeoutWithoutFallback,
+        TimeoutWithFallback,
+        PublicationValidationFailure,
+    }
+
+    private sealed class ScriptedPipelineSolver : IQpSolver
+    {
+        private readonly PipelineSolverMode mode;
+        private readonly PlanningGridMap? mapToCorrupt;
+        private int longitudinalCallCount;
+
+        public ScriptedPipelineSolver(PipelineSolverMode mode, PlanningGridMap? mapToCorrupt = null)
+        {
+            this.mode = mode;
+            this.mapToCorrupt = mapToCorrupt;
+        }
+
+        public QpSolveResult Solve(QuadraticProgram problem, QpSolverSettings settings, IReadOnlyList<double> warmStart,
+            CancellationToken cancellationToken)
+        {
+            bool longitudinal = problem.VariableCount > 100;
+            if (mode == PipelineSolverMode.SolverUnavailable)
+                return Result(QpSolveStatus.SolverUnavailable, Array.Empty<double>());
+            if (!longitudinal)
+            {
+                if (mode == PipelineSolverMode.LateralInfeasible)
+                    return Result(QpSolveStatus.PrimalInfeasible, Array.Empty<double>());
+                if (mode == PipelineSolverMode.TimeoutWithoutFallback)
+                    return Result(QpSolveStatus.TimeLimit, Array.Empty<double>());
+                return Result(QpSolveStatus.Solved, new double[problem.VariableCount]);
+            }
+            if (mode == PipelineSolverMode.LongitudinalInfeasible)
+                return Result(QpSolveStatus.PrimalInfeasible, Array.Empty<double>());
+            if (mode == PipelineSolverMode.PublicationValidationFailure && longitudinalCallCount == 0)
+                CorruptMapAtOrigin(mapToCorrupt);
+            if (mode == PipelineSolverMode.TimeoutWithFallback && ++longitudinalCallCount > 1)
+                return Result(QpSolveStatus.TimeLimit, Array.Empty<double>());
+            longitudinalCallCount++;
+            return Result(QpSolveStatus.Solved, CreateStrictLongitudinalPrimal(problem));
+        }
+
+        private static void CorruptMapAtOrigin(PlanningGridMap? map)
+        {
+            if (map == null || !map.TryWorldToGrid(0d, 0d, out int row, out int column))
+                throw new InvalidOperationException("Unable to corrupt the publication test map.");
+            FieldInfo occupiedField = typeof(PlanningGridMap).GetField("_occupied", BindingFlags.Instance | BindingFlags.NonPublic)!
+                ?? throw new InvalidOperationException("Planning map occupancy storage was unavailable.");
+            FieldInfo distanceField = typeof(PlanningGridMap).GetField("_conservativeDistances",
+                BindingFlags.Instance | BindingFlags.NonPublic) ?? throw new InvalidOperationException("Planning map distance storage was unavailable.");
+            byte[] occupied = (byte[])occupiedField.GetValue(map)!;
+            double[] distances = (double[])distanceField.GetValue(map)!;
+            int index = row * map.Cols + column;
+            occupied[index] = 1;
+            distances[index] = 0d;
+        }
+
+        private static double[] CreateStrictLongitudinalPrimal(QuadraticProgram problem)
+        {
+            int variableCount = problem.VariableCount;
+            int knotCount = (variableCount + 1) / 4;
+            var layout = new LongitudinalVariableLayout(knotCount);
+            var jerk = new double[knotCount - 1];
+            const int rampIntervals = 5;
+            for (int index = 0; index < rampIntervals; index++) jerk[index] = 1d;
+            for (int index = rampIntervals; index < 3 * rampIntervals; index++) jerk[index] = -1d;
+            for (int index = 3 * rampIntervals; index < 4 * rampIntervals; index++) jerk[index] = 1d;
+            var times = new double[knotCount];
+            for (int index = 0; index < times.Length; index++) times[index] = index * 0.05d;
+            LongitudinalCandidate baseCandidate = LongitudinalCandidate.Integrate(times, 0d, 0d, 0d, jerk);
+            double terminalPathS = ReadFixedVariable(problem, layout.S(knotCount - 1));
+            double scale = terminalPathS / baseCandidate.S[baseCandidate.S.Count - 1];
+            for (int index = 0; index < jerk.Length; index++) jerk[index] *= scale;
+            LongitudinalCandidate candidate = LongitudinalCandidate.Integrate(times, 0d, 0d, 0d, jerk);
+            var primal = new double[layout.VariableCount];
+            for (int index = 0; index < knotCount; index++)
+            {
+                primal[layout.S(index)] = candidate.S[index];
+                primal[layout.U(index)] = candidate.U[index];
+                primal[layout.A(index)] = candidate.A[index];
+            }
+            for (int index = 0; index < jerk.Length; index++) primal[layout.J(index)] = candidate.J[index];
+            for (int index = 4 * rampIntervals; index < knotCount; index++)
+            {
+                primal[layout.S(index)] = terminalPathS;
+                primal[layout.U(index)] = 0d;
+                primal[layout.A(index)] = 0d;
+            }
+            return primal;
+        }
+
+        private static double ReadFixedVariable(QuadraticProgram problem, int variable)
+        {
+            for (int row = 0; row < problem.ConstraintCount; row++)
+            {
+                int entryCount = 0;
+                double coefficient = 0d;
+                for (int column = 0; column < problem.ConstraintMatrix.ColumnCount; column++)
+                {
+                    for (int index = problem.ConstraintMatrix.ColumnPointers[column];
+                        index < problem.ConstraintMatrix.ColumnPointers[column + 1]; index++)
+                    {
+                        if (problem.ConstraintMatrix.RowIndices[index] != row)
+                            continue;
+                        entryCount++;
+                        if (column == variable)
+                            coefficient = problem.ConstraintMatrix.Values[index];
+                    }
+                }
+                if (entryCount == 1 && Math.Abs(coefficient) > 1e-12d &&
+                    Math.Abs(problem.LowerBounds[row] - problem.UpperBounds[row]) <= 1e-12d)
+                {
+                    return problem.LowerBounds[row] / coefficient;
+                }
+            }
+            throw new InvalidOperationException("Expected a fixed ST variable constraint.");
+        }
+
+        private static QpSolveResult Result(QpSolveStatus status, IReadOnlyList<double> primal)
+        {
+            return new QpSolveResult(status, primal, 0d, 0d, 0d, 1, TimeSpan.Zero, status.ToString(), string.Empty);
+        }
+    }
+
+    private sealed class ThrowingDebugSink : IEmPlannerDebugSink
+    {
+        public void Write(string message)
+        {
+            throw new InvalidOperationException("debug sink failure");
+        }
+    }
+}

ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs

@@ -11,7 +11,8 @@ internal static class Program
             args[0] != "all-foundation" && args[0] != "lateral-model" && args[0] != "lateral-integration" &&
             args[0] != "lateral-real-osqp" && args[0] != "lateral-real-osqp-probe" && args[0] != "lateral-all" &&
             args[0] != "longitudinal-model" && args[0] != "longitudinal-integration" &&
-            args[0] != "longitudinal-real-osqp-probe" && args[0] != "trajectory"))
+            args[0] != "longitudinal-real-osqp-probe" && args[0] != "trajectory" &&
+            args[0] != "em-planning-service" && args[0] != "em-core-all"))
         {
             Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|osqp|osqp-loader|all-foundation|lateral-model|lateral-integration|lateral-real-osqp|lateral-all|longitudinal-model|longitudinal-integration");
             return 2;
@@ -93,6 +94,20 @@ internal static class Program
                 MultiWheelC.TrajectoryPlanning.EMPlanner.TrajectoryChecks.Run();
                 Console.WriteLine("PASS trajectory");
             }
+            if (args[0] == "em-planning-service" || args[0] == "em-core-all")
+            {
+                if (args[0] == "em-core-all")
+                {
+                    MultiWheelC.TrajectoryPlanning.EMPlanner.LongitudinalModelChecks.Run();
+                    Console.WriteLine("PASS longitudinal-model");
+                    MultiWheelC.TrajectoryPlanning.EMPlanner.LongitudinalIntegrationChecks.Run();
+                    Console.WriteLine("PASS longitudinal-integration");
+                    MultiWheelC.TrajectoryPlanning.EMPlanner.TrajectoryChecks.Run();
+                    Console.WriteLine("PASS trajectory");
+                }
+                MultiWheelC.TrajectoryPlanning.EMPlanner.EmPlanningServiceChecks.Run();
+                Console.WriteLine("PASS em-planning-service");
+            }
             return 0;
         }
         catch (Exception exception)