feat: coordinate rolling EM replans
This commit is contained in:
梁薄云
@@ -0,0 +1,124 @@
|
||||
using System;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>
|
||||
/// 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.
|
||||
/// </summary>
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>Returns whether a caller-supplied time is due for another rolling cycle.</summary>
|
||||
public bool ShouldStartCycle(DateTimeOffset now)
|
||||
{
|
||||
lock (publicationGate)
|
||||
{
|
||||
return !lastCycleStartedAtUtc.HasValue ||
|
||||
now - lastCycleStartedAtUtc.Value >= TimeSpan.FromSeconds(replanPeriodSeconds);
|
||||
}
|
||||
}
|
||||
|
||||
public Task<PlanningCycleResult> 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,7 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Optional observer for completed rolling planning cycles.</summary>
|
||||
public interface IEmPlanningCycleSink
|
||||
{
|
||||
void OnCycleCompleted(PlanningCycleResult result);
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Caller-captured request and clock value for one rolling cycle.</summary>
|
||||
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;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,34 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
/// <summary>Immutable completion record for one rolling planning cycle.</summary>
|
||||
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);
|
||||
}
|
||||
}
|
||||
@@ -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");
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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)
|
||||
|
||||
Reference in New Issue
Block a user