219 lines
12 KiB
C#
219 lines
12 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
|
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
|
using MultiWheelC.TrajectoryPlanning.Utils;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
public sealed class EmPlanningRequestValidationResult
|
|
{
|
|
internal EmPlanningRequestValidationResult(EmPlanningStatus status, string failureReason, EmPlanningRequestSnapshot snapshot)
|
|
{
|
|
Status = status;
|
|
FailureReason = failureReason ?? string.Empty;
|
|
Snapshot = snapshot;
|
|
}
|
|
|
|
public EmPlanningStatus Status { get; }
|
|
|
|
public string FailureReason { get; }
|
|
|
|
public bool IsValid { get { return Status == EmPlanningStatus.Success; } }
|
|
|
|
internal EmPlanningRequestSnapshot Snapshot { get; }
|
|
}
|
|
|
|
internal sealed class EmPlanningRequestSnapshot
|
|
{
|
|
public EmPlanningRequestSnapshot(EmPlanningRequest request, EmPlannerConfiguration configuration)
|
|
{
|
|
Request = request;
|
|
Configuration = configuration;
|
|
}
|
|
|
|
public EmPlanningRequest Request { get; }
|
|
|
|
public EmPlannerConfiguration Configuration { get; }
|
|
}
|
|
|
|
public static class EmPlanningRequestValidator
|
|
{
|
|
public static EmPlanningRequestValidationResult Validate(EmPlanningRequest request)
|
|
{
|
|
if (request == null)
|
|
return Invalid("Request is required.");
|
|
if (request.ReferencePath == null || request.Map == null || request.Vehicle == null || request.VehicleState == null || request.Configuration == null)
|
|
return Invalid("Request members ReferencePath, Map, Vehicle, VehicleState, and Configuration are required.");
|
|
if (request.MotionModel == EmMotionModel.CrabTranslation || request.MotionModel == EmMotionModel.InPlaceRotation)
|
|
return Result(EmPlanningStatus.UnsupportedMotionMode, "Only nonholonomic forward/reverse motion is supported.");
|
|
if (request.MotionModel != EmMotionModel.NonholonomicForwardReverse)
|
|
return Invalid("Motion model is invalid.");
|
|
if (!Enum.IsDefined(typeof(EmPlanningScope), request.PlanningScope))
|
|
return Invalid("Planning scope is invalid.");
|
|
if (!TryValidateConfiguration(request.Configuration, request.PlanningScope, out string configurationFailure))
|
|
return Invalid(configurationFailure);
|
|
if (!request.Map.PlanningReady)
|
|
return Invalid("Planning map is not ready.");
|
|
if (!IsConsumable(request.ReferencePath.Status))
|
|
return Result(EmPlanningStatus.InvalidReferencePath, "Reference path status is not consumable.");
|
|
if (request.ReferencePath.Path == null || request.ReferencePath.Segments == null ||
|
|
request.SegmentIndex < 0 || request.SegmentIndex >= request.ReferencePath.Segments.Count)
|
|
return Result(EmPlanningStatus.InvalidReferencePath, "Requested direction segment is out of range.");
|
|
if (!IsValidVehicleState(request.VehicleState))
|
|
return Invalid("Vehicle state contains invalid values.");
|
|
if (!IsValidVehicle(request.Vehicle))
|
|
return Invalid("Vehicle geometry or curvature limit is invalid.");
|
|
if (request.RequestedAtUtc - request.VehicleState.CapturedAtUtc >
|
|
TimeSpan.FromSeconds(request.Configuration.Scheduling.MaximumVehicleStateAgeSeconds))
|
|
return Result(EmPlanningStatus.StaleVehicleState, "Vehicle state is stale.");
|
|
|
|
return new EmPlanningRequestValidationResult(
|
|
EmPlanningStatus.Success,
|
|
string.Empty,
|
|
new EmPlanningRequestSnapshot(request, request.Configuration.Copy()));
|
|
}
|
|
|
|
private static EmPlanningRequestValidationResult Invalid(string reason)
|
|
{
|
|
return Result(EmPlanningStatus.InvalidInput, reason);
|
|
}
|
|
|
|
private static EmPlanningRequestValidationResult Result(EmPlanningStatus status, string reason)
|
|
{
|
|
return new EmPlanningRequestValidationResult(status, reason, null);
|
|
}
|
|
|
|
private static bool IsConsumable(PathSmoothingStatus status)
|
|
{
|
|
return status == PathSmoothingStatus.Complete ||
|
|
status == PathSmoothingStatus.PartialImprovement ||
|
|
status == PathSmoothingStatus.NotNeeded ||
|
|
status == PathSmoothingStatus.Unchanged;
|
|
}
|
|
|
|
private static bool IsValidVehicleState(VehicleMotionState state)
|
|
{
|
|
return state.Pose != null &&
|
|
NumericGuard.IsFinite(state.Pose.X) &&
|
|
NumericGuard.IsFinite(state.Pose.Y) &&
|
|
NumericGuard.IsFinite(state.Pose.Heading) &&
|
|
NumericGuard.IsFinite(state.SignedLongitudinalSpeedMetersPerSecond) &&
|
|
(!state.LongitudinalAccelerationMetersPerSecondSquared.HasValue || NumericGuard.IsFinite(state.LongitudinalAccelerationMetersPerSecondSquared.Value)) &&
|
|
state.SequenceId >= 0;
|
|
}
|
|
|
|
private static bool IsValidVehicle(MultiWheelC.TrajectoryPlanning.CoarsePath.VehicleParameters vehicle)
|
|
{
|
|
return NumericGuard.IsPositiveFinite(vehicle.LengthMeters) &&
|
|
NumericGuard.IsPositiveFinite(vehicle.WidthMeters) &&
|
|
NumericGuard.IsFinite(vehicle.SafetyMarginMeters) && vehicle.SafetyMarginMeters >= 0d &&
|
|
VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out _);
|
|
}
|
|
|
|
private static bool TryValidateConfiguration(EmPlannerConfiguration configuration, EmPlanningScope planningScope,
|
|
out string failureReason)
|
|
{
|
|
failureReason = "Configuration is invalid.";
|
|
if (configuration.Scheduling == null || configuration.Corridor == null || configuration.Frenet == null ||
|
|
configuration.Lateral == null || configuration.Longitudinal == null || configuration.Solver == null ||
|
|
configuration.Validation == null || configuration.Lateral.Weights == null || configuration.Longitudinal.Weights == null)
|
|
return false;
|
|
|
|
SchedulingConfiguration scheduling = configuration.Scheduling;
|
|
CorridorConfiguration corridor = configuration.Corridor;
|
|
FrenetConfiguration frenet = configuration.Frenet;
|
|
LateralConfiguration lateral = configuration.Lateral;
|
|
LongitudinalConfiguration longitudinal = configuration.Longitudinal;
|
|
SolverConfiguration solver = configuration.Solver;
|
|
ValidationConfiguration validation = configuration.Validation;
|
|
|
|
if (!Positive(scheduling.ReplanPeriodSeconds) || !Positive(scheduling.TimeHorizonSeconds) || !Positive(scheduling.DistanceHorizonMeters) ||
|
|
!Positive(scheduling.OutputTimeStepSeconds) || !Positive(scheduling.SolverTimeoutSeconds) || !Positive(scheduling.HandoffLookaheadSeconds) || !Positive(scheduling.MaximumVehicleStateAgeSeconds) ||
|
|
!Positive(corridor.LongitudinalSampleSpacingMeters) || !Positive(corridor.LateralSampleSpacingMeters) || !Positive(corridor.MaximumLateralOffsetMeters) ||
|
|
!NonNegative(corridor.AdditionalClearanceReserveMeters) || !Positive(corridor.MaximumCollisionCheckStepMeters) ||
|
|
!Positive(frenet.MaximumProjectionDistanceMeters) || !NumericGuard.IsFinite(frenet.MinimumFrenetDenominator) ||
|
|
frenet.MinimumFrenetDenominator <= 0d || frenet.MinimumFrenetDenominator >= 1d || !Positive(frenet.BoundaryAnchorToleranceMeters) ||
|
|
!Positive(lateral.MaximumLateralStepPerIterationMeters) || !Positive(lateral.MaximumLateralSlope) ||
|
|
!Positive(lateral.MaximumLateralSecondDerivativePerMeter) || !Positive(lateral.MaximumLateralThirdDerivativePerSquareMeter) ||
|
|
!Positive(longitudinal.MaximumForwardSpeedMetersPerSecond) || !Positive(longitudinal.MaximumReverseSpeedMetersPerSecond) ||
|
|
!Positive(longitudinal.DesiredForwardSpeedMetersPerSecond) ||
|
|
longitudinal.DesiredForwardSpeedMetersPerSecond > longitudinal.MaximumForwardSpeedMetersPerSecond ||
|
|
!Positive(longitudinal.DesiredReverseSpeedMetersPerSecond) ||
|
|
longitudinal.DesiredReverseSpeedMetersPerSecond > longitudinal.MaximumReverseSpeedMetersPerSecond ||
|
|
!Positive(longitudinal.MaximumAccelerationMetersPerSecondSquared) || !Positive(longitudinal.MaximumDecelerationMetersPerSecondSquared) ||
|
|
!Positive(longitudinal.MaximumJerkMetersPerSecondCubed) || !Positive(longitudinal.MaximumLateralAccelerationMetersPerSecondSquared) ||
|
|
!Positive(longitudinal.MaximumCurvatureRatePerMeterPerSecond) || !NonNegative(longitudinal.StopSpeedToleranceMetersPerSecond) ||
|
|
!Positive(longitudinal.ZeroSpeedHoldSeconds) || solver.MaximumOuterIterations <= 0 || solver.MaximumOsqpIterations <= 0 ||
|
|
!Positive(solver.AbsoluteTolerance) || !Positive(solver.RelativeTolerance) || !Positive(solver.StrictResidualTolerance) ||
|
|
!Positive(validation.SpatialToleranceMeters) || !Positive(validation.KinematicTolerance) ||
|
|
!Positive(scheduling.MaximumOptimizationTimeStepSeconds) ||
|
|
!Positive(scheduling.MaximumOptimizationSpatialStepMeters) ||
|
|
scheduling.MaximumOptimizationKnotCount < 3 || scheduling.MaximumPublishedSampleCount < 2 ||
|
|
!Positive(validation.TerminalPositionToleranceMeters) ||
|
|
!Positive(validation.TerminalYawToleranceRadians) || validation.TerminalYawToleranceRadians > Math.PI ||
|
|
!WeightsAreValid(configuration.Lateral.Weights) || !WeightsAreValid(configuration.Longitudinal.Weights))
|
|
return false;
|
|
|
|
if (!JerkLimitedStoppingMath.TryCalculate(longitudinal.MaximumForwardSpeedMetersPerSecond,
|
|
longitudinal.MaximumAccelerationMetersPerSecondSquared,
|
|
longitudinal.MaximumDecelerationMetersPerSecondSquared,
|
|
longitudinal.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile forwardStop,
|
|
out _) ||
|
|
!JerkLimitedStoppingMath.TryCalculate(longitudinal.MaximumReverseSpeedMetersPerSecond,
|
|
longitudinal.MaximumAccelerationMetersPerSecondSquared,
|
|
longitudinal.MaximumDecelerationMetersPerSecondSquared,
|
|
longitudinal.MaximumJerkMetersPerSecondCubed, out JerkLimitedStoppingProfile reverseStop,
|
|
out _))
|
|
{
|
|
failureReason = "Configuration cannot construct the jerk-limited stopping model.";
|
|
return false;
|
|
}
|
|
|
|
double maximumSpeed = Math.Max(longitudinal.MaximumForwardSpeedMetersPerSecond,
|
|
longitudinal.MaximumReverseSpeedMetersPerSecond);
|
|
double requiredDistanceHorizon = Math.Max(forwardStop.DistanceMeters, reverseStop.DistanceMeters) +
|
|
maximumSpeed * scheduling.ReplanPeriodSeconds;
|
|
if (planningScope == EmPlanningScope.RollingHorizon &&
|
|
scheduling.DistanceHorizonMeters + 1e-12d < requiredDistanceHorizon)
|
|
{
|
|
failureReason = "DistanceHorizonMeters is insufficient: required=" + requiredDistanceHorizon +
|
|
";configured=" + scheduling.DistanceHorizonMeters;
|
|
return false;
|
|
}
|
|
|
|
try
|
|
{
|
|
IReadOnlyList<double> knotTimes = LongitudinalCandidate.CreateKnotTimes(
|
|
scheduling.TimeHorizonSeconds, scheduling.OutputTimeStepSeconds);
|
|
LongitudinalTerminalSchedule.GetStabilizationStartIndex(knotTimes,
|
|
scheduling.OutputTimeStepSeconds);
|
|
}
|
|
catch (ArgumentException exception)
|
|
{
|
|
failureReason = exception.Message;
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
private static bool Positive(double value) { return NumericGuard.IsPositiveFinite(value); }
|
|
|
|
private static bool NonNegative(double value) { return NumericGuard.IsFinite(value) && value >= 0d; }
|
|
|
|
private static bool WeightsAreValid(LateralWeights weights)
|
|
{
|
|
return NonNegative(weights.ReferenceOffset) && NonNegative(weights.HeadingDeviation) &&
|
|
NonNegative(weights.SecondDerivative) && NonNegative(weights.ThirdDerivative) &&
|
|
NonNegative(weights.Curvature) && NonNegative(weights.CurvatureVariation) &&
|
|
NonNegative(weights.PreviousTrajectory) && NonNegative(weights.RollingTerminal);
|
|
}
|
|
|
|
private static bool WeightsAreValid(LongitudinalWeights weights)
|
|
{
|
|
return NonNegative(weights.ReferenceSpeed) && NonNegative(weights.Acceleration) && NonNegative(weights.Jerk) &&
|
|
NonNegative(weights.PreviousTrajectory) && NonNegative(weights.TerminalAcceleration);
|
|
}
|
|
}
|