diff --git a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/EmPlanningCoordinator.cs b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/EmPlanningCoordinator.cs new file mode 100644 index 0000000..f479bab --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/EmPlanningCoordinator.cs @@ -0,0 +1,124 @@ +using System; +using System.Threading; +using System.Threading.Tasks; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// +/// Latest-wins rolling coordinator around the pure one-shot planning service. +/// It owns no localization, hardware, UI, or wall-clock source; callers supply each cycle's state and time. +/// +public sealed class EmPlanningCoordinator +{ + private readonly IEmPlanningService planningService; + private readonly IEmPlanningCycleSink sink; + private readonly object publicationGate = new object(); + private long latestCycleVersion; + private PlanningCycleIdentity latestIdentity; + private CancellationTokenSource latestCancellation; + private EmTrajectory publishedTrajectory; + private DateTimeOffset? lastCycleStartedAtUtc; + private double replanPeriodSeconds = 0.20d; + + public EmPlanningCoordinator(IEmPlanningService planningService, IEmPlanningCycleSink sink = null) + { + this.planningService = planningService ?? throw new ArgumentNullException(nameof(planningService)); + this.sink = sink; + } + + public EmTrajectory PublishedTrajectory + { + get + { + lock (publicationGate) + return publishedTrajectory; + } + } + + /// Returns whether a caller-supplied time is due for another rolling cycle. + public bool ShouldStartCycle(DateTimeOffset now) + { + lock (publicationGate) + { + return !lastCycleStartedAtUtc.HasValue || + now - lastCycleStartedAtUtc.Value >= TimeSpan.FromSeconds(replanPeriodSeconds); + } + } + + public Task PlanLatestAsync(PlanningCycleInput input, CancellationToken cancellationToken) + { + if (input == null) + throw new ArgumentNullException(nameof(input)); + + long version; + CancellationTokenSource cycleCancellation; + lock (publicationGate) + { + latestCancellation?.Cancel(); + latestCancellation = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken); + cycleCancellation = latestCancellation; + version = ++latestCycleVersion; + latestIdentity = input.Identity; + lastCycleStartedAtUtc = input.Now; + replanPeriodSeconds = input.ReplanPeriodSeconds; + } + + return Task.Run(() => CompleteCycle(version, input, cycleCancellation)); + } + + private PlanningCycleResult CompleteCycle(long version, PlanningCycleInput input, + CancellationTokenSource cycleCancellation) + { + EmPlanningResult planned; + try + { + planned = planningService.Plan(input.Request, cycleCancellation.Token); + if (planned == null) + planned = new EmPlanningResult(EmPlanningStatus.Failed, null, "planning service returned no result"); + } + catch (OperationCanceledException) + { + planned = new EmPlanningResult(EmPlanningStatus.Cancelled, null, "planning cycle cancelled"); + } + catch (Exception exception) + { + planned = new EmPlanningResult(EmPlanningStatus.Failed, null, "planning service exception: " + exception.Message); + } + + PlanningCycleResult result; + lock (publicationGate) + { + bool current = version == latestCycleVersion && input.Identity.Equals(latestIdentity); + if (!current) + { + var superseded = new EmPlanningResult(EmPlanningStatus.Superseded, null, + "rolling cycle superseded before publication"); + result = new PlanningCycleResult(version, input.Identity, superseded, false, superseded.FailureReason); + } + else + { + bool publishable = (planned.Status == EmPlanningStatus.Success || planned.Status == EmPlanningStatus.SuccessWithFallback) && + planned.Trajectory != null; + if (publishable) + publishedTrajectory = planned.Trajectory; + result = new PlanningCycleResult(version, input.Identity, planned, publishable, planned.FailureReason); + } + } + + if (sink == null) + return result; + + try + { + sink.OnCycleCompleted(result); + return result; + } + catch (Exception exception) + { + string diagnostic = string.IsNullOrEmpty(result.Diagnostic) + ? "cycle sink exception: " + exception.Message + : result.Diagnostic + "; cycle sink exception: " + exception.Message; + return result.WithDiagnostic(diagnostic); + } + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/IEmPlanningCycleSink.cs b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/IEmPlanningCycleSink.cs new file mode 100644 index 0000000..ce102c2 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/IEmPlanningCycleSink.cs @@ -0,0 +1,7 @@ +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Optional observer for completed rolling planning cycles. +public interface IEmPlanningCycleSink +{ + void OnCycleCompleted(PlanningCycleResult result); +} diff --git a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/PlanningCycleIdentity.cs b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/PlanningCycleIdentity.cs new file mode 100644 index 0000000..ba71ea1 --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/PlanningCycleIdentity.cs @@ -0,0 +1,75 @@ +using System; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Immutable identity bound to one rolling planning cycle. +public sealed class PlanningCycleIdentity : IEquatable +{ + 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; + } + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/PlanningCycleInput.cs b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/PlanningCycleInput.cs new file mode 100644 index 0000000..52b301d --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/PlanningCycleInput.cs @@ -0,0 +1,29 @@ +using System; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Caller-captured request and clock value for one rolling cycle. +public sealed class PlanningCycleInput +{ + public PlanningCycleInput(EmPlanningRequest request, DateTimeOffset now) + { + Request = request ?? throw new ArgumentNullException(nameof(request)); + Identity = PlanningCycleIdentity.FromRequest(request); + Now = now; + ReplanPeriodSeconds = ReadReplanPeriod(request.Configuration); + } + + public EmPlanningRequest Request { get; } + + public PlanningCycleIdentity Identity { get; } + + public DateTimeOffset Now { get; } + + public double ReplanPeriodSeconds { get; } + + private static double ReadReplanPeriod(EmPlannerConfiguration configuration) + { + double configured = configuration?.Scheduling?.ReplanPeriodSeconds ?? 0.20d; + return !double.IsNaN(configured) && !double.IsInfinity(configured) && configured > 0d ? configured : 0.20d; + } +} diff --git a/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/PlanningCycleResult.cs b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/PlanningCycleResult.cs new file mode 100644 index 0000000..075b4eb --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/TrajectoryExecution/PlanningCycleResult.cs @@ -0,0 +1,34 @@ +using System; + +namespace MultiWheelC.TrajectoryPlanning.EMPlanner; + +/// Immutable completion record for one rolling planning cycle. +public sealed class PlanningCycleResult +{ + public PlanningCycleResult(long version, PlanningCycleIdentity identity, EmPlanningResult result, bool published, + string diagnostic) + { + if (version <= 0) + throw new ArgumentOutOfRangeException(nameof(version)); + Version = version; + Identity = identity ?? throw new ArgumentNullException(nameof(identity)); + Result = result ?? throw new ArgumentNullException(nameof(result)); + Published = published; + Diagnostic = diagnostic ?? string.Empty; + } + + public long Version { get; } + + public PlanningCycleIdentity Identity { get; } + + public EmPlanningResult Result { get; } + + public bool Published { get; } + + public string Diagnostic { get; } + + internal PlanningCycleResult WithDiagnostic(string diagnostic) + { + return new PlanningCycleResult(Version, Identity, Result, Published, diagnostic); + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/CoordinatorChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/CoordinatorChecks.cs new file mode 100644 index 0000000..807f97a --- /dev/null +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/CoordinatorChecks.cs @@ -0,0 +1,222 @@ +using System; +using System.Collections.Generic; +using System.Threading; +using System.Threading.Tasks; +using MultiWheelC.TrajectoryPlanning.CoarsePath; +using MultiWheelC.TrajectoryPlanning.EMPlanner; +using MultiWheelC.TrajectoryPlanning.Mapping; + +namespace EMPlannerVerificationHost; + +internal static class CoordinatorChecks +{ + public static void Run() + { + VerifiesCallerSuppliedSchedulingDecision(); + VerifiesCompletedCycleDoesNotPoisonNextCancellationSource(); + VerifiesLatestCycleWinsAndEveryIdentityFieldSuppressesStaleResults(); + VerifiesSinkExceptionsAreIsolatedIntoCycleDiagnostics(); + } + + private static void VerifiesCallerSuppliedSchedulingDecision() + { + var service = new ControlledPlanningService(); + var coordinator = new EmPlanningCoordinator(service); + DateTimeOffset start = DateTimeOffset.UnixEpoch.AddSeconds(100d); + PlanningCycleInput input = CreateInput(CreateMap(1), "schedule-reference", 7L, "prior", 0, "schedule", start); + + Verification.True(coordinator.ShouldStartCycle(start), "first cycle is due"); + Task cycle = coordinator.PlanLatestAsync(input, CancellationToken.None); + service.WaitUntilStarted("schedule"); + service.Complete("schedule"); + Verification.Equal(EmPlanningStatus.Success, cycle.GetAwaiter().GetResult().Result.Status, "schedule setup result"); + + Verification.True(!coordinator.ShouldStartCycle(start.AddSeconds(0.199d)), "cycle remains deferred before period"); + Verification.True(coordinator.ShouldStartCycle(start.AddSeconds(0.20d)), "cycle becomes due at exact period"); + } + + private static void VerifiesLatestCycleWinsAndEveryIdentityFieldSuppressesStaleResults() + { + PlanningGridMap firstMap = CreateMap(10); + PlanningGridMap secondMap = CreateMap(11); + DateTimeOffset now = DateTimeOffset.UnixEpoch.AddSeconds(200d); + + VerifySuperseded("later-version", CreateInput(firstMap, "reference", 7L, "prior", 0, "later-version-A", now), + CreateInput(firstMap, "reference", 7L, "prior", 0, "later-version-B", now.AddSeconds(0.20d))); + VerifySuperseded("map", CreateInput(firstMap, "reference", 7L, "prior", 0, "map-A", now), + CreateInput(secondMap, "reference", 7L, "prior", 0, "map-B", now.AddSeconds(0.20d))); + VerifySuperseded("reference", CreateInput(firstMap, "reference-A", 7L, "prior", 0, "reference-A", now), + CreateInput(firstMap, "reference-B", 7L, "prior", 0, "reference-B", now.AddSeconds(0.20d))); + VerifySuperseded("state", CreateInput(firstMap, "reference", 7L, "prior", 0, "state-A", now), + CreateInput(firstMap, "reference", 8L, "prior", 0, "state-B", now.AddSeconds(0.20d))); + VerifySuperseded("segment", CreateInput(firstMap, "reference", 7L, "prior", 0, "segment-A", now), + CreateInput(firstMap, "reference", 7L, "prior", 1, "segment-B", now.AddSeconds(0.20d))); + VerifySuperseded("previous", CreateInput(firstMap, "reference", 7L, "prior-A", 0, "previous-A", now), + CreateInput(firstMap, "reference", 7L, "prior-B", 0, "previous-B", now.AddSeconds(0.20d))); + } + + private static void VerifiesCompletedCycleDoesNotPoisonNextCancellationSource() + { + var service = new ControlledPlanningService(); + var coordinator = new EmPlanningCoordinator(service); + DateTimeOffset now = DateTimeOffset.UnixEpoch.AddSeconds(150d); + + PlanningCycleInput first = CreateInput(CreateMap(5), "completed-reference", 4L, "prior", 0, + "completed-first", now); + Task firstCycle = coordinator.PlanLatestAsync(first, CancellationToken.None); + service.WaitUntilStarted(first.Request.OutputTrajectoryId); + service.Complete(first.Request.OutputTrajectoryId); + Verification.Equal(EmPlanningStatus.Success, firstCycle.GetAwaiter().GetResult().Result.Status, + "first completed cycle succeeds"); + + PlanningCycleInput second = CreateInput(CreateMap(6), "completed-reference", 5L, "completed-first", 0, + "completed-second", now.AddSeconds(0.20d)); + Task secondCycle = coordinator.PlanLatestAsync(second, CancellationToken.None); + service.WaitUntilStarted(second.Request.OutputTrajectoryId); + service.Complete(second.Request.OutputTrajectoryId); + + PlanningCycleResult result = secondCycle.GetAwaiter().GetResult(); + Verification.Equal(EmPlanningStatus.Success, result.Result.Status, "next cycle after completion succeeds"); + Verification.True(result.Published, "next cycle after completion publishes"); + } + + private static void VerifiesSinkExceptionsAreIsolatedIntoCycleDiagnostics() + { + var service = new ControlledPlanningService(); + var coordinator = new EmPlanningCoordinator(service, new ThrowingCycleSink()); + PlanningCycleInput input = CreateInput(CreateMap(20), "sink-reference", 9L, "prior", 0, "sink", DateTimeOffset.UnixEpoch); + + Task cycle = coordinator.PlanLatestAsync(input, CancellationToken.None); + service.WaitUntilStarted("sink"); + service.Complete("sink"); + PlanningCycleResult result = cycle.GetAwaiter().GetResult(); + + Verification.Equal(EmPlanningStatus.Success, result.Result.Status, "sink failure preserves planning result"); + Verification.True(result.Diagnostic.IndexOf("cycle sink", StringComparison.OrdinalIgnoreCase) >= 0, + "sink failure is reported in diagnostic"); + } + + private static void VerifySuperseded(string name, PlanningCycleInput older, PlanningCycleInput newer) + { + var service = new ControlledPlanningService(); + var coordinator = new EmPlanningCoordinator(service); + + Task slow = coordinator.PlanLatestAsync(older, CancellationToken.None); + service.WaitUntilStarted(older.Request.OutputTrajectoryId); + Task current = coordinator.PlanLatestAsync(newer, CancellationToken.None); + service.WaitUntilStarted(newer.Request.OutputTrajectoryId); + + Verification.True(service.WasCancelled(older.Request.OutputTrajectoryId), name + " cancels older cycle token"); + service.Complete(newer.Request.OutputTrajectoryId); + PlanningCycleResult currentResult = current.GetAwaiter().GetResult(); + service.Complete(older.Request.OutputTrajectoryId); + PlanningCycleResult staleResult = slow.GetAwaiter().GetResult(); + + Verification.Equal(EmPlanningStatus.Success, currentResult.Result.Status, name + " current result succeeds"); + Verification.True(currentResult.Published, name + " current result publishes"); + Verification.Equal(EmPlanningStatus.Superseded, staleResult.Result.Status, name + " old result is superseded"); + Verification.True(staleResult.Result.Trajectory == null, name + " stale result exposes no trajectory"); + Verification.Equal(newer.Request.OutputTrajectoryId, coordinator.PublishedTrajectory.Metadata.TrajectoryId, + name + " only latest trajectory is published"); + } + + private static PlanningCycleInput CreateInput(PlanningGridMap map, string referencePathId, long stateSequenceId, + string previousTrajectoryId, int segmentIndex, string outputTrajectoryId, DateTimeOffset now) + { + EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault(); + var state = new VehicleMotionState(new Pose2D(0d, 0d, 0d), 0d, 0d, now, stateSequenceId); + var request = new EmPlanningRequest(null, map, null, state, configuration, segmentIndex, null, now, now, + outputTrajectoryId, referencePathId, previousTrajectoryId, EmMotionModel.NonholonomicForwardReverse); + return new PlanningCycleInput(request, now); + } + + private static PlanningGridMap CreateMap(int widthOffset) + { + PlanningMapBuildResult build = new PlanningMapFactory().Create(new PlanningMapRequest + { + Bounds = new MapBoundsMm(-1000f, 1000f + widthOffset * 20f, -1000f, 1000f), + ResolutionMm = 20f, + ObstacleSources = Array.Empty(), + AllowExplicitEmptyMap = true, + }); + Verification.True(build.Succeeded && build.Map != null && build.Map.PlanningReady, "coordinator map builds"); + return build.Map!; + } + + private sealed class ControlledPlanningService : IEmPlanningService + { + private readonly object gate = new object(); + private readonly Dictionary pending = new Dictionary(); + + public EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken) + { + var cycle = new PendingCycle(request, cancellationToken); + lock (gate) + { + pending.Add(request.OutputTrajectoryId, cycle); + Monitor.PulseAll(gate); + } + return cycle.Completion.Task.GetAwaiter().GetResult(); + } + + public void WaitUntilStarted(string outputTrajectoryId) + { + DateTimeOffset timeout = DateTimeOffset.UtcNow.AddSeconds(5d); + lock (gate) + { + while (!pending.ContainsKey(outputTrajectoryId)) + { + TimeSpan remaining = timeout - DateTimeOffset.UtcNow; + if (remaining <= TimeSpan.Zero || !Monitor.Wait(gate, remaining)) + throw new InvalidOperationException("Planner cycle did not start: " + outputTrajectoryId); + } + } + } + + public bool WasCancelled(string outputTrajectoryId) + { + lock (gate) + return pending[outputTrajectoryId].CancellationToken.IsCancellationRequested; + } + + public void Complete(string outputTrajectoryId) + { + PendingCycle cycle; + lock (gate) + cycle = pending[outputTrajectoryId]; + cycle.Completion.TrySetResult(CreateSuccess(cycle.Request)); + } + + private static EmPlanningResult CreateSuccess(EmPlanningRequest request) + { + var metadata = new EmTrajectoryMetadata(request.OutputTrajectoryId, request.RequestedAtUtc, request.EffectiveAtUtc, + request.Map.SnapshotId, request.ReferencePathId, request.VehicleState.SequenceId, request.PreviousTrajectoryId, + request.SegmentIndex, TravelDirection.Forward, EmTerminalType.RollingSafetyStop); + var point = new EmTrajectoryPoint(0d, 0d, 0d, 0d, 0d, 0d, request.SegmentIndex, 0d, 0d, + TravelDirection.Forward, EmBoundaryType.RollingSafetyStop, 0d, 0d); + return new EmPlanningResult(EmPlanningStatus.Success, new EmTrajectory(metadata, new[] { point }), string.Empty); + } + + private sealed class PendingCycle + { + public PendingCycle(EmPlanningRequest request, CancellationToken cancellationToken) + { + Request = request; + CancellationToken = cancellationToken; + Completion = new TaskCompletionSource(TaskCreationOptions.RunContinuationsAsynchronously); + } + + public EmPlanningRequest Request { get; } + public CancellationToken CancellationToken { get; } + public TaskCompletionSource Completion { get; } + } + } + + private sealed class ThrowingCycleSink : IEmPlanningCycleSink + { + public void OnCycleCompleted(PlanningCycleResult result) + { + throw new InvalidOperationException("cycle sink failure"); + } + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs index 3bbfb33..87e5061 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs @@ -12,7 +12,7 @@ internal static class Program 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] != "em-planning-service" && args[0] != "em-core-all")) + args[0] != "em-planning-service" && args[0] != "em-core-all" && args[0] != "coordinator")) { 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; @@ -108,6 +108,11 @@ internal static class Program MultiWheelC.TrajectoryPlanning.EMPlanner.EmPlanningServiceChecks.Run(); Console.WriteLine("PASS em-planning-service"); } + if (args[0] == "coordinator") + { + CoordinatorChecks.Run(); + Console.WriteLine("PASS coordinator"); + } return 0; } catch (Exception exception)