feat: validate EM terminal world pose

2026-08-07 08:01:18 +08:00
parent 0bba8d7e61
commit e844fe00a8
6 changed files with 249 additions and 11 deletions
@@ -124,15 +124,22 @@ public sealed class EmPlanningService : IEmPlanningService
                request.Map.SnapshotId, request.ReferencePathId, request.VehicleState.SequenceId, request.PreviousTrajectoryId,
                segment.SegmentIndex, segment.Direction, horizon.TerminalType, horizon.LongitudinalMode,
                request.PlanningScope);
-            EmTrajectory trajectory = new EmTrajectoryAssembler(configuration).Assemble(lateral.Path, longitudinal, metadata);
+            EmPlanningStatus assemblyStatus = new EmTrajectoryAssembler(configuration).TryAssemble(lateral.Path,
+                longitudinal, metadata, out EmTrajectory trajectory, out string assemblyFailure);
+            if (assemblyStatus != EmPlanningStatus.Success)
+                return Failure(assemblyStatus, request, assemblyFailure);
            EmitDebug(request, "trajectory assembly succeeded");

+            EmBoundaryType terminalBoundary = slice.TerminalBoundary.BoundaryType;
+            Pose2D terminalPose = IsRealTerminalBoundary(terminalBoundary)
+                ? TerminalPose(lateral.Path)
+                : null;
            EmTrajectoryValidationResult publication = new EmTrajectoryValidator().Validate(trajectory, request.Map,
                request.Vehicle, configuration, segment.SegmentIndex, longitudinalInput.PathUpperBoundS,
-                slice.TerminalBoundary.BoundaryType);
+                terminalPose, terminalBoundary);
            if (!publication.IsValid)
            {
-                return Failure(EmPlanningStatus.ValidationFailed, request,
+                return Failure(MapPublicationFailure(publication.Failure), request,
                    publication.Failure + " at point " + publication.PointIndex + ": " + publication.Message);
            }
            EmitDebug(request, "world-space publication validation succeeded");
@@ -225,6 +232,24 @@ public sealed class EmPlanningService : IEmPlanningService
        return status == EmPlanningStatus.Success || status == EmPlanningStatus.SuccessWithFallback;
    }

+    internal static EmPlanningStatus MapPublicationFailure(EmTrajectoryValidationFailure failure)
+    {
+        return failure == EmTrajectoryValidationFailure.TerminalPoseMismatch
+            ? EmPlanningStatus.TerminalPoseMismatch
+            : EmPlanningStatus.ValidationFailed;
+    }
+
+    private static bool IsRealTerminalBoundary(EmBoundaryType boundaryType)
+    {
+        return boundaryType == EmBoundaryType.Goal || boundaryType == EmBoundaryType.GearSwitchApproach;
+    }
+
+    private static Pose2D TerminalPose(LateralPath path)
+    {
+        LateralPathPoint terminal = path.Points[path.Points.Count - 1];
+        return new Pose2D(terminal.X, terminal.Y, terminal.VehicleYaw);
+    }
+
    private static EmPlanningResult Failure(EmPlanningStatus status, EmPlanningRequest request, string reason)
    {
        return new EmPlanningResult(status, null, DiagnosticsPrefix(request) + ";reason=" + (reason ?? string.Empty));