feat: coordinate rolling EM replans

2026-08-04 12:47:39 +08:00
parent 49109ec835
commit d75380cf8f
7 changed files with 497 additions and 1 deletions
@@ -0,0 +1,75 @@
+using System;
+
+namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
+
+/// <summary>Immutable identity bound to one rolling planning cycle.</summary>
+public sealed class PlanningCycleIdentity : IEquatable<PlanningCycleIdentity>
+{
+    public PlanningCycleIdentity(long mapSnapshotId, string referencePathId, long vehicleStateSequenceId,
+        string previousTrajectoryId, int segmentIndex)
+    {
+        if (mapSnapshotId < 0)
+            throw new ArgumentOutOfRangeException(nameof(mapSnapshotId));
+        if (string.IsNullOrWhiteSpace(referencePathId))
+            throw new ArgumentException("A reference path ID is required.", nameof(referencePathId));
+        if (vehicleStateSequenceId < 0)
+            throw new ArgumentOutOfRangeException(nameof(vehicleStateSequenceId));
+        if (segmentIndex < 0)
+            throw new ArgumentOutOfRangeException(nameof(segmentIndex));
+
+        MapSnapshotId = mapSnapshotId;
+        ReferencePathId = referencePathId;
+        VehicleStateSequenceId = vehicleStateSequenceId;
+        PreviousTrajectoryId = previousTrajectoryId ?? string.Empty;
+        SegmentIndex = segmentIndex;
+    }
+
+    public long MapSnapshotId { get; }
+
+    public string ReferencePathId { get; }
+
+    public long VehicleStateSequenceId { get; }
+
+    public string PreviousTrajectoryId { get; }
+
+    public int SegmentIndex { get; }
+
+    public static PlanningCycleIdentity FromRequest(EmPlanningRequest request)
+    {
+        if (request == null)
+            throw new ArgumentNullException(nameof(request));
+        if (request.Map == null)
+            throw new ArgumentException("A planning map is required for a rolling cycle.", nameof(request));
+        if (request.VehicleState == null)
+            throw new ArgumentException("A vehicle state is required for a rolling cycle.", nameof(request));
+
+        return new PlanningCycleIdentity(request.Map.SnapshotId, request.ReferencePathId,
+            request.VehicleState.SequenceId, request.PreviousTrajectoryId, request.SegmentIndex);
+    }
+
+    public bool Equals(PlanningCycleIdentity other)
+    {
+        return other != null && MapSnapshotId == other.MapSnapshotId &&
+            string.Equals(ReferencePathId, other.ReferencePathId, StringComparison.Ordinal) &&
+            VehicleStateSequenceId == other.VehicleStateSequenceId &&
+            string.Equals(PreviousTrajectoryId, other.PreviousTrajectoryId, StringComparison.Ordinal) &&
+            SegmentIndex == other.SegmentIndex;
+    }
+
+    public override bool Equals(object obj)
+    {
+        return Equals(obj as PlanningCycleIdentity);
+    }
+
+    public override int GetHashCode()
+    {
+        unchecked
+        {
+            int hash = MapSnapshotId.GetHashCode();
+            hash = (hash * 397) ^ StringComparer.Ordinal.GetHashCode(ReferencePathId);
+            hash = (hash * 397) ^ VehicleStateSequenceId.GetHashCode();
+            hash = (hash * 397) ^ StringComparer.Ordinal.GetHashCode(PreviousTrajectoryId);
+            return (hash * 397) ^ SegmentIndex;
+        }
+    }
+}