217 lines
12 KiB
C#
217 lines
12 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
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 readonly IQpSolver qpSolver;
|
|
private readonly IEmPlannerDebugSink defaultDebugSink;
|
|
|
|
public EmPlanningService(IQpSolver qpSolver, IEmPlannerDebugSink defaultDebugSink = null)
|
|
{
|
|
this.qpSolver = qpSolver ?? throw new ArgumentNullException(nameof(qpSolver));
|
|
this.defaultDebugSink = defaultDebugSink;
|
|
}
|
|
|
|
public EmPlanningResult Plan(EmPlanningRequest request, CancellationToken cancellationToken)
|
|
{
|
|
if (cancellationToken.IsCancellationRequested)
|
|
return Failure(EmPlanningStatus.Cancelled, request, "Planning was cancelled before request validation.");
|
|
|
|
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");
|
|
|
|
try
|
|
{
|
|
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();
|
|
if (!projector.TryProject(request.VehicleState.Pose, segment, 0d, segment.LengthMeters,
|
|
configuration.Frenet.MaximumProjectionDistanceMeters, 0d, out FrenetProjection startProjection))
|
|
{
|
|
return Failure(EmPlanningStatus.ProjectionFailed, request,
|
|
"Vehicle pose could not be projected inside the selected direction segment.");
|
|
}
|
|
EmitDebug(request, "bounded ego projection succeeded");
|
|
|
|
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, 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");
|
|
|
|
var lateralInput = new LateralPlanningInput(segment, corridor, startProjection, horizon.TerminalType,
|
|
request.Vehicle, configuration, previousSeed);
|
|
LateralPlanningResult lateral = new LateralPlanner(qpSolver).Plan(lateralInput, cancellationToken);
|
|
if (!IsSuccess(lateral.Status))
|
|
return Failure(lateral.Status, request, lateral.FailureReason);
|
|
EmitDebug(request, "LS optimization and validation succeeded");
|
|
|
|
IReadOnlyList<double> knotTimes = LongitudinalCandidate.CreateKnotTimes(
|
|
configuration.Scheduling.TimeHorizonSeconds, configuration.Scheduling.OutputTimeStepSeconds);
|
|
LongitudinalPreviousTrajectorySeed previousLongitudinalSeed =
|
|
new LongitudinalPreviousTrajectorySeedBuilder().Build(
|
|
request.PreviousTrajectory, lateral.Path, request.EffectiveAtUtc, knotTimes,
|
|
segment.SegmentIndex, segment.Direction);
|
|
var longitudinalInput = new LongitudinalPlanningInput(lateral.Path, segment.Direction, initialProgressSpeed,
|
|
initialAcceleration, horizon.TerminalType, horizon.LongitudinalMode, configuration,
|
|
previousLongitudinalSeed.PathS, previousLongitudinalSeed.ProgressSpeedMetersPerSecond);
|
|
EmPlanningStatus envelopeStatus = new PathSpeedLimitBuilder().Build(longitudinalInput, out _, out string envelopeReason);
|
|
if (envelopeStatus != EmPlanningStatus.Success)
|
|
return Failure(envelopeStatus, request, envelopeReason);
|
|
EmitDebug(request, "PathS speed envelope succeeded");
|
|
|
|
LongitudinalPlanningResult longitudinal = new LongitudinalPlanner(qpSolver).Plan(longitudinalInput, cancellationToken);
|
|
if (!IsSuccess(longitudinal.Status))
|
|
return Failure(longitudinal.Status, request, longitudinal.FailureReason);
|
|
EmitDebug(request, "ST optimization and validation succeeded");
|
|
|
|
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);
|
|
EmTrajectory trajectory = new EmTrajectoryAssembler(configuration).Assemble(lateral.Path, longitudinal, metadata);
|
|
EmitDebug(request, "trajectory assembly succeeded");
|
|
|
|
EmTrajectoryValidationResult publication = new EmTrajectoryValidator().Validate(trajectory, request.Map,
|
|
request.Vehicle, configuration, segment.SegmentIndex, longitudinalInput.PathUpperBoundS,
|
|
slice.TerminalBoundary.BoundaryType);
|
|
if (!publication.IsValid)
|
|
{
|
|
return Failure(EmPlanningStatus.ValidationFailed, request,
|
|
publication.Failure + " at point " + publication.PointIndex + ": " + publication.Message);
|
|
}
|
|
EmitDebug(request, "world-space publication validation succeeded");
|
|
|
|
EmPlanningStatus finalStatus = lateral.Status == EmPlanningStatus.SuccessWithFallback ||
|
|
longitudinal.Status == EmPlanningStatus.SuccessWithFallback
|
|
? EmPlanningStatus.SuccessWithFallback
|
|
: EmPlanningStatus.Success;
|
|
return new EmPlanningResult(finalStatus, trajectory,
|
|
DiagnosticsPrefix(request) + ";terminal=" + horizon.TerminalType + ";publication=validated");
|
|
}
|
|
catch (OperationCanceledException)
|
|
{
|
|
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 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 bool IsSuccess(EmPlanningStatus status)
|
|
{
|
|
return status == EmPlanningStatus.Success || status == EmPlanningStatus.SuccessWithFallback;
|
|
}
|
|
|
|
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;
|
|
}
|
|
}
|