From 019b89645a0d5c15b77695010417b78909e47843 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E6=A2=81=E8=96=84=E4=BA=91?= Date: Tue, 4 Aug 2026 11:55:20 +0800 Subject: [PATCH] feat: publish one-shot EM trajectories --- .../EMPlanner/Facade/EmPlanningService.cs | 209 ++++++++++ .../EMPlanner/Facade/IEmPlanningService.cs | 9 + .../ParkrobTrajplanner/EMPlanner/README.md | 129 ++++++ .../EmPlanningServiceChecks.cs | 367 ++++++++++++++++++ .../EMPlannerVerificationHost/Program.cs | 17 +- 5 files changed, 730 insertions(+), 1 deletion(-) create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs create mode 100644 ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/IEmPlanningService.cs create mode 100644 ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/EmPlanningService.cs new file mode 100644 index 0000000..410faf7 --- /dev/null +++ b/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; + +/// Runs one deterministic EM LS/ST planning pipeline and publishes only independently validated trajectories. +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 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 previousSeed = ProjectPreviousTrajectorySeed(request.PreviousTrajectory, segment, + startProjection.ReferenceS, horizon.TerminalReferenceS, configuration.Frenet.MaximumProjectionDistanceMeters); + var corridorSeed = new List(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(), Array.Empty()); + 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 ProjectPreviousTrajectorySeed(EmTrajectory previousTrajectory, + DirectionSegmentView segment, double minimumReferenceS, double maximumReferenceS, double maximumDistanceMeters) + { + var projected = new List(); + 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; + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/IEmPlanningService.cs b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/IEmPlanningService.cs new file mode 100644 index 0000000..25b5115 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/Facade/IEmPlanningService.cs @@ -0,0 +1,9 @@ +using System.Threading; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Pure, one-shot EM planning boundary with no scheduler, UI, hardware, or clock dependency. +public interface IEmPlanningService +{ + EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken); +} diff --git a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md b/ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md index fa7ac22..536d52b 100644 --- a/ClumsyPilot/ParkrobTrajplanner/EMPlanner/README.md +++ b/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=;reference=;state=; +previous=;segment= +``` + +### 结果、字段和单位 + +`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 +``` diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/EmPlanningServiceChecks.cs new file mode 100644 index 0000000..4652c1a --- /dev/null +++ b/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 + { + 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 + { + 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()); + } + + 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(); + 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 warmStart, + CancellationToken cancellationToken) + { + bool longitudinal = problem.VariableCount > 100; + if (mode == PipelineSolverMode.SolverUnavailable) + return Result(QpSolveStatus.SolverUnavailable, Array.Empty()); + if (!longitudinal) + { + if (mode == PipelineSolverMode.LateralInfeasible) + return Result(QpSolveStatus.PrimalInfeasible, Array.Empty()); + if (mode == PipelineSolverMode.TimeoutWithoutFallback) + return Result(QpSolveStatus.TimeLimit, Array.Empty()); + return Result(QpSolveStatus.Solved, new double[problem.VariableCount]); + } + if (mode == PipelineSolverMode.LongitudinalInfeasible) + return Result(QpSolveStatus.PrimalInfeasible, Array.Empty()); + if (mode == PipelineSolverMode.PublicationValidationFailure && longitudinalCallCount == 0) + CorruptMapAtOrigin(mapToCorrupt); + if (mode == PipelineSolverMode.TimeoutWithFallback && ++longitudinalCallCount > 1) + return Result(QpSolveStatus.TimeLimit, Array.Empty()); + 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 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"); + } + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs index 8acb1e0..3bbfb33 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs +++ b/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)