456 lines
25 KiB
C#
456 lines
25 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using System.Diagnostics;
|
|
using System.Globalization;
|
|
using System.Threading;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
/// <summary>Runs one deterministic EM LS/ST planning pipeline and publishes only independently validated trajectories.</summary>
|
|
public sealed class EmPlanningService : IEmPlanningService
|
|
{
|
|
private static readonly TimeSpan MaximumCycleDeadlineRemaining =
|
|
TimeSpan.FromMilliseconds(int.MaxValue);
|
|
private readonly IQpSolver qpSolver;
|
|
private readonly IEmPlannerDebugSink defaultDebugSink;
|
|
private readonly Func<TimeSpan> cycleElapsedForTesting;
|
|
|
|
public EmPlanningService(IQpSolver qpSolver, IEmPlannerDebugSink defaultDebugSink = null)
|
|
: this(qpSolver, defaultDebugSink, null)
|
|
{
|
|
}
|
|
|
|
internal EmPlanningService(IQpSolver qpSolver, IEmPlannerDebugSink defaultDebugSink,
|
|
Func<TimeSpan> cycleElapsedForTesting)
|
|
{
|
|
this.qpSolver = qpSolver ?? throw new ArgumentNullException(nameof(qpSolver));
|
|
this.defaultDebugSink = defaultDebugSink;
|
|
this.cycleElapsedForTesting = cycleElapsedForTesting;
|
|
}
|
|
|
|
public EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken)
|
|
{
|
|
Stopwatch cycleStopwatch = cycleElapsedForTesting == null ? Stopwatch.StartNew() : null;
|
|
Func<TimeSpan> cycleElapsed = cycleElapsedForTesting ?? (() => cycleStopwatch.Elapsed);
|
|
if (CallerCancellationRequested(request, cancellationToken))
|
|
return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled before request validation.");
|
|
if (request?.CycleDeadlineRemaining is TimeSpan suppliedRemaining &&
|
|
(suppliedRemaining < TimeSpan.Zero || suppliedRemaining > MaximumCycleDeadlineRemaining))
|
|
{
|
|
return Failure(EmPlanningStatus.InvalidInput, request,
|
|
"CycleDeadlineRemaining must be between zero and " +
|
|
MaximumCycleDeadlineRemaining.TotalMilliseconds + " milliseconds.");
|
|
}
|
|
if (DeadlineExpired(request, cycleElapsed))
|
|
return CycleDeadlineFailure(request, "request", CycleRemaining(request, cycleElapsed));
|
|
|
|
EmPlanningRequestValidationResult requestValidation = EmPlanningRequestValidator.Validate(request);
|
|
if (!requestValidation.IsValid)
|
|
return Failure(requestValidation.Status, request, requestValidation.FailureReason);
|
|
EmPlannerConfiguration configuration = requestValidation.Snapshot.Configuration;
|
|
EmitDebug(request, "request/config validation succeeded");
|
|
if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
|
|
"request-validation", out EmPlanningResult validationFailure))
|
|
{
|
|
return validationFailure;
|
|
}
|
|
TimeSpan expirationRemaining = CycleRemaining(request, cycleElapsed);
|
|
using var cycleExpiration = request.CycleDeadlineRemaining.HasValue
|
|
? new CancellationTokenSource(expirationRemaining)
|
|
: null;
|
|
using var linkedCancellation = CancellationTokenSource.CreateLinkedTokenSource(
|
|
cancellationToken, request.CallerCancellationToken, request.CycleDeadlineToken,
|
|
cycleExpiration?.Token ?? CancellationToken.None);
|
|
CancellationToken planningCancellationToken = linkedCancellation.Token;
|
|
|
|
try
|
|
{
|
|
if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
|
|
"projection", out EmPlanningResult deadlineFailure))
|
|
return deadlineFailure;
|
|
IReadOnlyList<DirectionSegmentView> segments = ReferencePathSegmenter.Create(request.ReferencePath);
|
|
if (request.SegmentIndex < 0 || request.SegmentIndex >= segments.Count)
|
|
return Failure(EmPlanningStatus.InvalidReferencePath, request, "The requested direction segment is unavailable.");
|
|
DirectionSegmentView segment = segments[request.SegmentIndex];
|
|
if (!HasCompatibleStateDirection(request.VehicleState, segment.Direction,
|
|
configuration.Longitudinal.StopSpeedToleranceMetersPerSecond))
|
|
{
|
|
return Failure(EmPlanningStatus.StateDirectionMismatch, request,
|
|
"Vehicle signed speed contradicts the selected direction segment.");
|
|
}
|
|
EmitDebug(request, "direction-segment selection succeeded");
|
|
|
|
var projector = new FrenetProjector();
|
|
double startProjectionUpperS = request.PlanningScope == EmPlanningScope.FullDirectionSegment
|
|
? Math.Min(segment.LengthMeters, configuration.Frenet.MaximumProjectionDistanceMeters)
|
|
: segment.LengthMeters;
|
|
if (!projector.TryProject(request.VehicleState.Pose, segment, 0d, startProjectionUpperS,
|
|
configuration.Frenet.MaximumProjectionDistanceMeters, 0d, out FrenetProjection startProjection))
|
|
{
|
|
return Failure(EmPlanningStatus.ProjectionFailed, request,
|
|
"Vehicle pose could not be projected at an admissible start of the selected direction segment.");
|
|
}
|
|
if (Math.Abs(startProjection.HeadingError) >= Math.PI / 2d)
|
|
{
|
|
return Failure(EmPlanningStatus.ProjectionFailed, request,
|
|
"Vehicle travel heading differs by at least 90 degrees from the selected direction segment.");
|
|
}
|
|
EmitDebug(request, "bounded ego projection succeeded");
|
|
if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "projection", out deadlineFailure))
|
|
return deadlineFailure;
|
|
|
|
if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "corridor", out deadlineFailure))
|
|
return deadlineFailure;
|
|
double initialProgressSpeed = Math.Abs(request.VehicleState.SignedLongitudinalSpeedMetersPerSecond);
|
|
double initialAcceleration = request.VehicleState.LongitudinalAccelerationMetersPerSecondSquared ?? 0d;
|
|
var horizonSelector = new PlanningHorizonSelector();
|
|
EmPlanningStatus horizonStatus = horizonSelector.Select(segment, startProjection.ReferenceS, initialProgressSpeed,
|
|
initialAcceleration, request.PlanningScope, configuration, out PlanningHorizonSelection horizon,
|
|
out string horizonReason);
|
|
if (horizonStatus != EmPlanningStatus.Success)
|
|
return Failure(horizonStatus, request, horizonReason);
|
|
ReferenceHorizonSlice slice = ReferenceHorizonSlicer.Slice(segment, horizon.WindowEndReferenceS);
|
|
EmitDebug(request, "exact horizon and terminal selection succeeded");
|
|
|
|
IReadOnlyList<FrenetProjection> previousSeed = ProjectPreviousTrajectorySeed(request.PreviousTrajectory, segment,
|
|
startProjection.ReferenceS, horizon.WindowEndReferenceS, configuration.Frenet.MaximumProjectionDistanceMeters);
|
|
var corridorSeed = new List<FrenetProjection>(previousSeed.Count + 1) { startProjection };
|
|
for (int index = 0; index < previousSeed.Count; index++) corridorSeed.Add(previousSeed[index]);
|
|
EmitDebug(request, "previous-trajectory seed projection completed");
|
|
|
|
var corridorBuilder = new StaticCorridorBuilder();
|
|
if (!corridorBuilder.TryBuild(segment, startProjection.ReferenceS, slice.TerminalBoundary.SegmentLocalS, corridorSeed,
|
|
request.Map, request.Vehicle, configuration.Corridor, out StaticCorridor corridor, out string corridorReason))
|
|
{
|
|
return Failure(EmPlanningStatus.CorridorInfeasible, request, corridorReason);
|
|
}
|
|
EmitDebug(request, "static connected corridor succeeded");
|
|
if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "corridor", out deadlineFailure))
|
|
return deadlineFailure;
|
|
|
|
if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "lateral", out deadlineFailure))
|
|
return deadlineFailure;
|
|
TimeSpan lateralBudget = SmallerBudget(
|
|
TimeSpan.FromSeconds(configuration.Scheduling.SolverTimeoutSeconds),
|
|
CycleRemaining(request, cycleElapsed));
|
|
EmPlannerConfiguration lateralConfiguration = configuration.Copy();
|
|
lateralConfiguration.Scheduling.SolverTimeoutSeconds = lateralBudget.TotalSeconds;
|
|
var lateralInput = new LateralPlanningInput(segment, corridor, startProjection, horizon.TerminalType,
|
|
request.Vehicle, lateralConfiguration, previousSeed);
|
|
TimeSpan totalSolveBudget = lateralBudget;
|
|
var solveBudgetStopwatch = Stopwatch.StartNew();
|
|
LateralPlanningResult lateral = new LateralPlanner(qpSolver).Plan(lateralInput, planningCancellationToken);
|
|
if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "lateral", out deadlineFailure))
|
|
return deadlineFailure;
|
|
if (!IsSuccess(lateral.Status))
|
|
return Failure(lateral.Status, request, lateral.FailureReason);
|
|
if (cancellationToken.IsCancellationRequested)
|
|
return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled after LS optimization.");
|
|
EmitDebug(request, "LS optimization and validation succeeded");
|
|
|
|
if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "envelope", out deadlineFailure))
|
|
return deadlineFailure;
|
|
EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(lateral.Path, segment.Direction,
|
|
initialProgressSpeed, initialAcceleration, horizon.TerminalType, configuration, out PathSpeedLimit speedLimit,
|
|
out string envelopeReason);
|
|
if (envelopeStatus != EmPlanningStatus.Success)
|
|
return Failure(envelopeStatus, request, envelopeReason);
|
|
LongitudinalKnotSchedule knotSchedule;
|
|
if (request.PlanningScope == EmPlanningScope.FullDirectionSegment)
|
|
{
|
|
EmPlanningStatus scheduleStatus = new FullDirectionSegmentScheduleBuilder().TryBuild(lateral.Path, speedLimit,
|
|
initialProgressSpeed, initialAcceleration, DesiredSpeed(configuration, segment.Direction), configuration,
|
|
out knotSchedule, out string scheduleReason);
|
|
if (scheduleStatus != EmPlanningStatus.Success)
|
|
return Failure(scheduleStatus, request, scheduleReason);
|
|
}
|
|
else
|
|
{
|
|
knotSchedule = LongitudinalKnotSchedule.CreateRolling(configuration.Scheduling.TimeHorizonSeconds,
|
|
configuration.Scheduling.OutputTimeStepSeconds);
|
|
}
|
|
LongitudinalPreviousTrajectorySeed previousLongitudinalSeed =
|
|
new LongitudinalPreviousTrajectorySeedBuilder().Build(
|
|
request.PreviousTrajectory, lateral.Path, request.EffectiveAtUtc, knotSchedule,
|
|
segment.SegmentIndex, segment.Direction);
|
|
if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "envelope", out deadlineFailure))
|
|
return deadlineFailure;
|
|
TimeSpan remainingSolveBudget = totalSolveBudget - solveBudgetStopwatch.Elapsed;
|
|
remainingSolveBudget = SmallerBudget(remainingSolveBudget, CycleRemaining(request, cycleElapsed));
|
|
if (remainingSolveBudget <= TimeSpan.Zero)
|
|
{
|
|
if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
|
|
"longitudinal", out deadlineFailure))
|
|
return deadlineFailure;
|
|
return Failure(EmPlanningStatus.SolverTimedOut, request,
|
|
"LS/ST optimization exhausted the shared solve budget before ST optimization.");
|
|
}
|
|
EmPlannerConfiguration longitudinalConfiguration = configuration.Copy();
|
|
longitudinalConfiguration.Scheduling.SolverTimeoutSeconds = remainingSolveBudget.TotalSeconds;
|
|
var longitudinalInput = new LongitudinalPlanningInput(lateral.Path, segment.Direction, initialProgressSpeed,
|
|
initialAcceleration, horizon.TerminalType, horizon.LongitudinalMode, longitudinalConfiguration,
|
|
request.PlanningScope, knotSchedule,
|
|
previousLongitudinalSeed.PathS, previousLongitudinalSeed.ProgressSpeedMetersPerSecond);
|
|
envelopeStatus = new PathSpeedLimitBuilder().Build(longitudinalInput, out _, out envelopeReason);
|
|
if (envelopeStatus != EmPlanningStatus.Success)
|
|
return Failure(envelopeStatus, request, envelopeReason);
|
|
EmitDebug(request, "PathS speed envelope succeeded");
|
|
|
|
if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
|
|
"longitudinal", out deadlineFailure))
|
|
return deadlineFailure;
|
|
LongitudinalPlanningResult longitudinal = new LongitudinalPlanner(qpSolver).Plan(
|
|
longitudinalInput, planningCancellationToken);
|
|
if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
|
|
"longitudinal", out deadlineFailure))
|
|
return deadlineFailure;
|
|
if (!IsSuccess(longitudinal.Status))
|
|
return Failure(longitudinal.Status, request, longitudinal.FailureReason);
|
|
if (cancellationToken.IsCancellationRequested)
|
|
return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled after ST optimization.");
|
|
EmitDebug(request, "ST optimization and validation succeeded");
|
|
|
|
if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "assembly", out deadlineFailure))
|
|
return deadlineFailure;
|
|
var metadata = new EmTrajectoryMetadata(request.OutputTrajectoryId, request.RequestedAtUtc, request.EffectiveAtUtc,
|
|
request.Map.SnapshotId, request.ReferencePathId, request.VehicleState.SequenceId, request.PreviousTrajectoryId,
|
|
segment.SegmentIndex, segment.Direction, horizon.TerminalType, horizon.LongitudinalMode,
|
|
request.PlanningScope);
|
|
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);
|
|
if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "assembly", out deadlineFailure))
|
|
return deadlineFailure;
|
|
if (cancellationToken.IsCancellationRequested)
|
|
return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled after trajectory assembly.");
|
|
EmitDebug(request, "trajectory assembly succeeded");
|
|
|
|
if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
|
|
"world-validation", out deadlineFailure))
|
|
return deadlineFailure;
|
|
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, segment.LengthMeters, longitudinalInput.PathUpperBoundS,
|
|
terminalPose, terminalBoundary);
|
|
if (!publication.IsValid)
|
|
{
|
|
return Failure(MapPublicationFailure(publication.Failure), request,
|
|
publication.Failure + " at point " + publication.PointIndex + ": " + publication.Message);
|
|
}
|
|
if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
|
|
"world-validation", out deadlineFailure))
|
|
return deadlineFailure;
|
|
EmitDebug(request, "world-space publication validation succeeded");
|
|
|
|
if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "publication", out deadlineFailure))
|
|
return deadlineFailure;
|
|
if (cancellationToken.IsCancellationRequested)
|
|
return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled before trajectory publication.");
|
|
|
|
EmPlanningStatus finalStatus = lateral.Status == EmPlanningStatus.SuccessWithFallback ||
|
|
longitudinal.Status == EmPlanningStatus.SuccessWithFallback
|
|
? EmPlanningStatus.SuccessWithFallback
|
|
: EmPlanningStatus.Success;
|
|
string publicationDiagnostic = DiagnosticsPrefix(request) + ";terminal=" + horizon.TerminalType +
|
|
";publication=validated";
|
|
if (lateral.Status == EmPlanningStatus.SuccessWithFallback)
|
|
publicationDiagnostic += ";lateralFallback=" + lateral.FailureReason;
|
|
if (!string.IsNullOrWhiteSpace(longitudinal.FailureReason))
|
|
{
|
|
publicationDiagnostic += longitudinal.Status == EmPlanningStatus.SuccessWithFallback
|
|
? ";longitudinalFallback=" + longitudinal.FailureReason
|
|
: ";longitudinal=" + longitudinal.FailureReason;
|
|
}
|
|
if (TryTerminalFailure(request, cancellationToken, cycleElapsed, "publication", out deadlineFailure))
|
|
return deadlineFailure;
|
|
return new EmPlanningResult(finalStatus, trajectory, publicationDiagnostic);
|
|
}
|
|
catch (OperationCanceledException)
|
|
{
|
|
if (TryTerminalFailure(request, cancellationToken, cycleElapsed,
|
|
"operation", out EmPlanningResult deadlineFailure))
|
|
return deadlineFailure;
|
|
return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled.");
|
|
}
|
|
catch (ArgumentException exception)
|
|
{
|
|
return Failure(EmPlanningStatus.Failed, request, exception.Message);
|
|
}
|
|
catch (InvalidOperationException exception)
|
|
{
|
|
return Failure(EmPlanningStatus.Failed, request, exception.Message);
|
|
}
|
|
}
|
|
|
|
private static bool TryTerminalFailure(EmPlanningRequest request, CancellationToken cancellationToken,
|
|
Func<TimeSpan> elapsed, string phase, out EmPlanningResult failure)
|
|
{
|
|
if (CallerCancellationRequested(request, cancellationToken))
|
|
{
|
|
failure = Failure(EmPlanningStatus.Cancelled, request,
|
|
"callerCancellation=true;phase=" + phase);
|
|
return true;
|
|
}
|
|
TimeSpan remaining = CycleRemaining(request, elapsed);
|
|
if (!DeadlineExpired(request, elapsed))
|
|
{
|
|
failure = null;
|
|
return false;
|
|
}
|
|
failure = CycleDeadlineFailure(request, phase, remaining);
|
|
return true;
|
|
}
|
|
|
|
private static bool CallerCancellationRequested(EmPlanningRequest request,
|
|
CancellationToken cancellationToken)
|
|
{
|
|
if (request?.CallerCancellationToken.IsCancellationRequested == true)
|
|
return true;
|
|
if (!cancellationToken.IsCancellationRequested)
|
|
return false;
|
|
return request == null || !request.CycleDeadlineToken.IsCancellationRequested ||
|
|
!request.CallerCancellationToken.CanBeCanceled;
|
|
}
|
|
|
|
private static bool DeadlineExpired(EmPlanningRequest request, Func<TimeSpan> elapsed)
|
|
{
|
|
return request != null && (request.CycleDeadlineToken.IsCancellationRequested ||
|
|
request.IsCycleDeadlineExpired() ||
|
|
request.CycleDeadlineRemaining.HasValue && CycleRemaining(request, elapsed) <= TimeSpan.Zero);
|
|
}
|
|
|
|
private static TimeSpan CycleRemaining(EmPlanningRequest request, Func<TimeSpan> elapsed)
|
|
{
|
|
TimeSpan snapshotRemaining = TimeSpan.MaxValue;
|
|
if (request.CycleDeadlineRemaining.HasValue)
|
|
{
|
|
snapshotRemaining = request.CycleDeadlineRemaining.Value - elapsed();
|
|
if (snapshotRemaining < TimeSpan.Zero)
|
|
snapshotRemaining = TimeSpan.Zero;
|
|
}
|
|
TimeSpan? dynamicRemaining = request.DynamicCycleDeadlineRemaining();
|
|
if (!dynamicRemaining.HasValue)
|
|
return snapshotRemaining;
|
|
return dynamicRemaining.Value < snapshotRemaining ? dynamicRemaining.Value : snapshotRemaining;
|
|
}
|
|
|
|
private static TimeSpan SmallerBudget(TimeSpan configured, TimeSpan cycleRemaining)
|
|
{
|
|
return configured < cycleRemaining ? configured : cycleRemaining;
|
|
}
|
|
|
|
private static EmPlanningResult CycleDeadlineFailure(EmPlanningRequest request, string phase,
|
|
TimeSpan remaining)
|
|
{
|
|
return Failure(EmPlanningStatus.CycleDeadlineExpired, request,
|
|
"cycleDeadlineExpired=true;phase=" + phase + ";remainingMs=" +
|
|
remaining.TotalMilliseconds.ToString("F3", CultureInfo.InvariantCulture));
|
|
}
|
|
|
|
private void EmitDebug(EmPlanningRequest request, string message)
|
|
{
|
|
if (defaultDebugSink == null || request == null || request.Configuration == null || request.Configuration.Solver == null ||
|
|
!request.Configuration.Solver.NativeVerbose)
|
|
{
|
|
return;
|
|
}
|
|
try
|
|
{
|
|
defaultDebugSink.Write(message ?? string.Empty);
|
|
}
|
|
catch
|
|
{
|
|
// Debug output is deliberately isolated from pure planning results.
|
|
}
|
|
}
|
|
|
|
private static IReadOnlyList<FrenetProjection> ProjectPreviousTrajectorySeed(EmTrajectory previousTrajectory,
|
|
DirectionSegmentView segment, double minimumReferenceS, double maximumReferenceS, double maximumDistanceMeters)
|
|
{
|
|
var projected = new List<FrenetProjection>();
|
|
if (previousTrajectory == null || previousTrajectory.Metadata.SegmentIndex != segment.SegmentIndex ||
|
|
previousTrajectory.Metadata.Direction != segment.Direction)
|
|
{
|
|
return projected;
|
|
}
|
|
|
|
var projector = new FrenetProjector();
|
|
double seedReferenceS = minimumReferenceS;
|
|
for (int index = 0; index < previousTrajectory.Points.Count; index++)
|
|
{
|
|
EmTrajectoryPoint point = previousTrajectory.Points[index];
|
|
if (point == null || point.TimeFromStart <= 0d || point.Direction != segment.Direction)
|
|
continue;
|
|
if (projector.TryProject(new Pose2D(point.X, point.Y, point.Yaw), segment, minimumReferenceS,
|
|
maximumReferenceS, maximumDistanceMeters, seedReferenceS, out FrenetProjection projection))
|
|
{
|
|
projected.Add(projection);
|
|
seedReferenceS = projection.ReferenceS;
|
|
}
|
|
}
|
|
return projected;
|
|
}
|
|
|
|
private static bool HasCompatibleStateDirection(VehicleMotionState state, TravelDirection direction, double stopTolerance)
|
|
{
|
|
if (state == null || double.IsNaN(stopTolerance) || double.IsInfinity(stopTolerance) || stopTolerance < 0d)
|
|
return false;
|
|
if (Math.Abs(state.SignedLongitudinalSpeedMetersPerSecond) <= stopTolerance)
|
|
return true;
|
|
return direction == TravelDirection.Forward
|
|
? state.SignedLongitudinalSpeedMetersPerSecond > 0d
|
|
: state.SignedLongitudinalSpeedMetersPerSecond < 0d;
|
|
}
|
|
|
|
private static double DesiredSpeed(EmPlannerConfiguration configuration, TravelDirection direction)
|
|
{
|
|
return direction == TravelDirection.Forward
|
|
? configuration.Longitudinal.DesiredForwardSpeedMetersPerSecond
|
|
: configuration.Longitudinal.DesiredReverseSpeedMetersPerSecond;
|
|
}
|
|
|
|
private static bool IsSuccess(EmPlanningStatus status)
|
|
{
|
|
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));
|
|
}
|
|
|
|
private static string DiagnosticsPrefix(EmPlanningRequest request)
|
|
{
|
|
if (request == null)
|
|
return "map=;reference=;state=;previous=;segment=";
|
|
return "map=" + (request.Map == null ? string.Empty : request.Map.SnapshotId.ToString()) +
|
|
";reference=" + (request.ReferencePathId ?? string.Empty) +
|
|
";state=" + (request.VehicleState == null ? string.Empty : request.VehicleState.SequenceId.ToString()) +
|
|
";previous=" + (request.PreviousTrajectoryId ?? string.Empty) +
|
|
";segment=" + request.SegmentIndex + ";scope=" + request.PlanningScope;
|
|
}
|
|
}
|