First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat: coordinate rolling EM replans

Browse Source
This commit is contained in:
梁薄云
2026-08-04 12:47:39 +08:00
parent 49109ec835
commit d75380cf8f
7 changed files with 497 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/tests/EMPlannerVerificationHost/CoordinatorChecks.cs
+222
View File
@@ -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<PlanningCycleResult> 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<PlanningCycleResult> 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<PlanningCycleResult> 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<PlanningCycleResult> 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<PlanningCycleResult> slow = coordinator.PlanLatestAsync(older, CancellationToken.None);
service.WaitUntilStarted(older.Request.OutputTrajectoryId);
Task<PlanningCycleResult> 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<IMapObstacleSource>(),
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<string, PendingCycle> pending = new Dictionary<string, PendingCycle>();
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<EmPlanningResult>(TaskCreationOptions.RunContinuationsAsynchronously);
}
public EmPlanningRequest Request { get; }
public CancellationToken CancellationToken { get; }
public TaskCompletionSource<EmPlanningResult> Completion { get; }
}
}
private sealed class ThrowingCycleSink : IEmPlanningCycleSink
{
public void OnCycleCompleted(PlanningCycleResult result)
{
throw new InvalidOperationException("cycle sink failure");
}
}
}
ClumsyPilot/tests/EMPlannerVerificationHost/Program.cs
+6 -1
View File
@@ -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)