360 lines
18 KiB
C#
360 lines
18 KiB
C#
using System;
|
|
using System.Globalization;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
|
using MultiWheelC.TrajectoryPlanning.CoarsePath.Vehicle;
|
|
using MultiWheelC.TrajectoryPlanning.Mapping;
|
|
|
|
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
|
|
|
public enum EmTrajectoryValidationFailure
|
|
{
|
|
None,
|
|
InvalidInput,
|
|
NonFinite,
|
|
TimeNotStrictlyIncreasing,
|
|
PathSDecreased,
|
|
SegmentBoundaryExceeded,
|
|
MissingTerminalAnchor,
|
|
TerminalSpeedNotZero,
|
|
TerminalYawRateNotZero,
|
|
DirectionMismatch,
|
|
DirectionSignMismatch,
|
|
RedundantSpeedMismatch,
|
|
WorldVelocityMismatch,
|
|
YawRateMismatch,
|
|
SpeedLimitExceeded,
|
|
AccelerationLimitExceeded,
|
|
JerkLimitExceeded,
|
|
CurvatureLimitExceeded,
|
|
CurvatureRateLimitExceeded,
|
|
PoseCollision,
|
|
SweptCollision,
|
|
}
|
|
|
|
public sealed class EmTrajectoryValidationResult
|
|
{
|
|
private EmTrajectoryValidationResult(EmTrajectoryValidationFailure failure, int pointIndex, string message)
|
|
{
|
|
Failure = failure;
|
|
PointIndex = pointIndex;
|
|
Message = message ?? string.Empty;
|
|
}
|
|
|
|
public bool IsValid { get { return Failure == EmTrajectoryValidationFailure.None; } }
|
|
|
|
public EmTrajectoryValidationFailure Failure { get; }
|
|
|
|
public int PointIndex { get; }
|
|
|
|
public string Message { get; }
|
|
|
|
internal static EmTrajectoryValidationResult Success()
|
|
{
|
|
return new EmTrajectoryValidationResult(EmTrajectoryValidationFailure.None, -1, string.Empty);
|
|
}
|
|
|
|
internal static EmTrajectoryValidationResult Reject(EmTrajectoryValidationFailure failure, int pointIndex,
|
|
string message)
|
|
{
|
|
return new EmTrajectoryValidationResult(failure, pointIndex, message);
|
|
}
|
|
}
|
|
|
|
/// <summary>Independently checks the public world-space trajectory before it can be published.</summary>
|
|
public sealed class EmTrajectoryValidator
|
|
{
|
|
private const double MaximumSweptCollisionStepMeters = 0.025d;
|
|
private readonly FootprintCollisionChecker collisionChecker;
|
|
|
|
public EmTrajectoryValidator()
|
|
: this(new FootprintCollisionChecker())
|
|
{
|
|
}
|
|
|
|
public EmTrajectoryValidator(FootprintCollisionChecker collisionChecker)
|
|
{
|
|
this.collisionChecker = collisionChecker ?? throw new ArgumentNullException(nameof(collisionChecker));
|
|
}
|
|
|
|
public EmTrajectoryValidationResult Validate(EmTrajectory trajectory, PlanningGridMap map, VehicleParameters vehicle,
|
|
EmPlannerConfiguration configuration, int segmentIndex, double terminalPathS, EmBoundaryType terminalBoundary)
|
|
{
|
|
if (trajectory == null || map == null || vehicle == null || configuration == null || configuration.Validation == null ||
|
|
configuration.Longitudinal == null || configuration.Corridor == null || segmentIndex < 0 ||
|
|
!IsFinite(terminalPathS) || terminalPathS < 0d || !Enum.IsDefined(typeof(EmBoundaryType), terminalBoundary) ||
|
|
!TryReadLimits(configuration, vehicle, trajectory.Metadata.Direction, out ValidationLimits limits))
|
|
{
|
|
return EmTrajectoryValidationResult.Reject(EmTrajectoryValidationFailure.InvalidInput, -1,
|
|
"Trajectory publication inputs or validation limits are invalid.");
|
|
}
|
|
|
|
for (int index = 0; index < trajectory.Points.Count; index++)
|
|
{
|
|
EmTrajectoryPoint point = trajectory.Points[index];
|
|
if (point == null || !HasOnlyFiniteValues(point))
|
|
return Reject(EmTrajectoryValidationFailure.NonFinite, index, "Trajectory contains a non-finite point.");
|
|
if (point.SegmentIndex != segmentIndex || point.SegmentLocalS > terminalPathS + limits.SpatialTolerance ||
|
|
point.PathS > terminalPathS + limits.SpatialTolerance)
|
|
{
|
|
return Reject(EmTrajectoryValidationFailure.SegmentBoundaryExceeded, index,
|
|
"Trajectory point lies outside the current direction segment.");
|
|
}
|
|
if (index == 0)
|
|
continue;
|
|
|
|
EmTrajectoryPoint previous = trajectory.Points[index - 1];
|
|
if (point.TimeFromStart <= previous.TimeFromStart)
|
|
return Reject(EmTrajectoryValidationFailure.TimeNotStrictlyIncreasing, index,
|
|
"Trajectory time must be strictly increasing.");
|
|
if (point.PathS + limits.SpatialTolerance < previous.PathS ||
|
|
point.SegmentLocalS + limits.SpatialTolerance < previous.SegmentLocalS)
|
|
{
|
|
return Reject(EmTrajectoryValidationFailure.PathSDecreased, index,
|
|
"Trajectory PathS must not decrease.");
|
|
}
|
|
}
|
|
|
|
int terminalIndex = FindTerminalAnchor(trajectory, terminalPathS, terminalBoundary, limits.SpatialTolerance);
|
|
if (terminalIndex < 0)
|
|
{
|
|
return Reject(EmTrajectoryValidationFailure.MissingTerminalAnchor,
|
|
FindFirstTerminalPathIndex(trajectory, terminalPathS, limits.SpatialTolerance),
|
|
"Trajectory does not contain the exact terminal boundary anchor.");
|
|
}
|
|
|
|
EmTrajectoryPoint terminal = trajectory.Points[terminalIndex];
|
|
if (Math.Abs(terminal.SignedLongitudinalVelocity) > limits.KinematicTolerance)
|
|
return Reject(EmTrajectoryValidationFailure.TerminalSpeedNotZero, terminalIndex,
|
|
"Terminal signed speed must be zero.");
|
|
if (Math.Abs(terminal.YawRate) > limits.KinematicTolerance)
|
|
return Reject(EmTrajectoryValidationFailure.TerminalYawRateNotZero, terminalIndex,
|
|
"Terminal yaw rate must be zero.");
|
|
|
|
double directionSign = trajectory.Metadata.Direction == TravelDirection.Forward ? 1d : -1d;
|
|
double previousAcceleration = 0d;
|
|
bool hasPreviousAcceleration = false;
|
|
for (int index = 0; index < trajectory.Points.Count; index++)
|
|
{
|
|
EmTrajectoryPoint point = trajectory.Points[index];
|
|
if (point.Direction != trajectory.Metadata.Direction)
|
|
return Reject(EmTrajectoryValidationFailure.DirectionMismatch, index,
|
|
"Trajectory point direction differs from trajectory metadata.");
|
|
if (Math.Abs(point.SignedLongitudinalVelocity) > limits.KinematicTolerance &&
|
|
point.SignedLongitudinalVelocity * directionSign < 0d)
|
|
{
|
|
return Reject(EmTrajectoryValidationFailure.DirectionSignMismatch, index,
|
|
"Trajectory signed speed has the wrong direction sign.");
|
|
}
|
|
if (!NearlyEqual(Math.Abs(point.SignedLongitudinalVelocity), point.Speed, limits.KinematicTolerance))
|
|
return Reject(EmTrajectoryValidationFailure.RedundantSpeedMismatch, index,
|
|
"Trajectory Speed is inconsistent with signed speed.");
|
|
if (!NearlyEqual(point.SignedLongitudinalVelocity * Math.Cos(point.Yaw), point.VelocityX,
|
|
limits.KinematicTolerance) ||
|
|
!NearlyEqual(point.SignedLongitudinalVelocity * Math.Sin(point.Yaw), point.VelocityY,
|
|
limits.KinematicTolerance))
|
|
{
|
|
return Reject(EmTrajectoryValidationFailure.WorldVelocityMismatch, index,
|
|
"Trajectory world velocity is inconsistent with signed speed and yaw.");
|
|
}
|
|
if (!NearlyEqual(point.SignedLongitudinalVelocity * point.VehicleCurvature, point.YawRate,
|
|
limits.KinematicTolerance))
|
|
{
|
|
return Reject(EmTrajectoryValidationFailure.YawRateMismatch, index,
|
|
"Trajectory yaw rate is inconsistent with signed speed and curvature.");
|
|
}
|
|
if (point.Speed > limits.MaximumSpeed + limits.KinematicTolerance)
|
|
return Reject(EmTrajectoryValidationFailure.SpeedLimitExceeded, index,
|
|
"Trajectory speed exceeds its direction limit.");
|
|
if (Math.Abs(point.VehicleCurvature) > limits.MaximumCurvature + limits.KinematicTolerance)
|
|
return Reject(EmTrajectoryValidationFailure.CurvatureLimitExceeded, index,
|
|
"Trajectory vehicle curvature exceeds the vehicle limit.");
|
|
|
|
if (index == 0)
|
|
continue;
|
|
|
|
EmTrajectoryPoint previous = trajectory.Points[index - 1];
|
|
double dt = point.TimeFromStart - previous.TimeFromStart;
|
|
double previousProgressSpeed = directionSign * previous.SignedLongitudinalVelocity;
|
|
double progressSpeed = directionSign * point.SignedLongitudinalVelocity;
|
|
double acceleration = (progressSpeed - previousProgressSpeed) / dt;
|
|
if (acceleration > limits.MaximumAcceleration + limits.KinematicTolerance ||
|
|
-acceleration > limits.MaximumDeceleration + limits.KinematicTolerance)
|
|
{
|
|
return Reject(EmTrajectoryValidationFailure.AccelerationLimitExceeded, index,
|
|
"Trajectory finite-difference acceleration exceeds its limit.");
|
|
}
|
|
double finiteDifferenceJerk = (acceleration - previousAcceleration) / dt;
|
|
if (hasPreviousAcceleration && Math.Abs(finiteDifferenceJerk) > limits.MaximumJerk +
|
|
limits.KinematicTolerance)
|
|
{
|
|
double storedPreviousJerk = index >= 2 ? trajectory.Points[index - 2].LongitudinalJerk : 0d;
|
|
double storedCurrentJerk = previous.LongitudinalJerk;
|
|
return Reject(EmTrajectoryValidationFailure.JerkLimitExceeded, index,
|
|
"Trajectory finite-difference jerk exceeds its limit" +
|
|
" (time=" + Format(point.TimeFromStart) + "s, dt=" + Format(dt) + "s" +
|
|
", previousAcceleration=" + Format(previousAcceleration) + "m/s2" +
|
|
", acceleration=" + Format(acceleration) + "m/s2" +
|
|
", jerk=" + Format(finiteDifferenceJerk) + "m/s3" +
|
|
", limit=" + Format(limits.MaximumJerk) + "m/s3" +
|
|
", excess=" + Format(Math.Abs(finiteDifferenceJerk) - limits.MaximumJerk) + "m/s3" +
|
|
", storedPreviousJerk=" + Format(storedPreviousJerk) + "m/s3" +
|
|
", storedCurrentJerk=" + Format(storedCurrentJerk) + "m/s3).");
|
|
}
|
|
if (Math.Abs(point.VehicleCurvature - previous.VehicleCurvature) / dt > limits.MaximumCurvatureRate +
|
|
limits.KinematicTolerance)
|
|
{
|
|
return Reject(EmTrajectoryValidationFailure.CurvatureRateLimitExceeded, index,
|
|
"Trajectory finite-difference curvature rate exceeds its limit.");
|
|
}
|
|
previousAcceleration = acceleration;
|
|
hasPreviousAcceleration = true;
|
|
}
|
|
|
|
for (int index = 0; index < trajectory.Points.Count; index++)
|
|
{
|
|
EmTrajectoryPoint point = trajectory.Points[index];
|
|
if (!collisionChecker.IsPoseCollisionFree(new Pose2D(point.X, point.Y, point.Yaw), map, vehicle, 0d, out _))
|
|
return Reject(EmTrajectoryValidationFailure.PoseCollision, index,
|
|
"Trajectory point fails the full-body world-space collision check.");
|
|
}
|
|
double sweptStepMeters = Math.Min(MaximumSweptCollisionStepMeters, limits.ConfiguredCollisionStepMeters);
|
|
for (int index = 1; index < trajectory.Points.Count; index++)
|
|
{
|
|
EmTrajectoryPoint previous = trajectory.Points[index - 1];
|
|
EmTrajectoryPoint point = trajectory.Points[index];
|
|
if (!collisionChecker.IsSweptMotionCollisionFree(new Pose2D(previous.X, previous.Y, previous.Yaw),
|
|
new Pose2D(point.X, point.Y, point.Yaw), map, vehicle, sweptStepMeters, out _))
|
|
{
|
|
return Reject(EmTrajectoryValidationFailure.SweptCollision, index,
|
|
"Trajectory segment fails the full-body swept world-space collision check.");
|
|
}
|
|
}
|
|
|
|
return EmTrajectoryValidationResult.Success();
|
|
}
|
|
|
|
private static int FindTerminalAnchor(EmTrajectory trajectory, double terminalPathS, EmBoundaryType terminalBoundary,
|
|
double spatialTolerance)
|
|
{
|
|
for (int index = 0; index < trajectory.Points.Count; index++)
|
|
{
|
|
EmTrajectoryPoint point = trajectory.Points[index];
|
|
if (point.BoundaryType == terminalBoundary && Math.Abs(point.PathS - terminalPathS) <= spatialTolerance)
|
|
return index;
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
private static int FindFirstTerminalPathIndex(EmTrajectory trajectory, double terminalPathS, double spatialTolerance)
|
|
{
|
|
for (int index = 0; index < trajectory.Points.Count; index++)
|
|
{
|
|
if (Math.Abs(trajectory.Points[index].PathS - terminalPathS) <= spatialTolerance)
|
|
return index;
|
|
}
|
|
return trajectory.Points.Count - 1;
|
|
}
|
|
|
|
private static bool HasOnlyFiniteValues(EmTrajectoryPoint point)
|
|
{
|
|
return IsFinite(point.X) && IsFinite(point.Y) && IsFinite(point.Yaw) &&
|
|
IsFinite(point.SignedLongitudinalVelocity) && IsFinite(point.Speed) && IsFinite(point.VelocityX) &&
|
|
IsFinite(point.VelocityY) && IsFinite(point.YawRate) && IsFinite(point.TimeFromStart) &&
|
|
IsFinite(point.VehicleCurvature) && IsFinite(point.SegmentLocalS) && IsFinite(point.PathS) &&
|
|
IsFinite(point.LongitudinalAcceleration) && IsFinite(point.LongitudinalJerk);
|
|
}
|
|
|
|
private static bool TryReadLimits(EmPlannerConfiguration configuration, VehicleParameters vehicle,
|
|
TravelDirection direction, out ValidationLimits limits)
|
|
{
|
|
limits = default;
|
|
double maximumCurvature;
|
|
if (vehicle.MaximumCurvaturePerMeter.HasValue)
|
|
maximumCurvature = vehicle.MaximumCurvaturePerMeter.Value;
|
|
else if (vehicle.MinimumTurningRadiusMeters.HasValue && vehicle.MinimumTurningRadiusMeters.Value > 0d)
|
|
maximumCurvature = 1d / vehicle.MinimumTurningRadiusMeters.Value;
|
|
else
|
|
return false;
|
|
|
|
if (direction != TravelDirection.Forward && direction != TravelDirection.Reverse)
|
|
return false;
|
|
double maximumSpeed = direction == TravelDirection.Forward
|
|
? configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond
|
|
: configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond;
|
|
if (!IsFinite(maximumSpeed) || maximumSpeed <= 0d || !IsFinite(maximumCurvature) || maximumCurvature <= 0d ||
|
|
!IsFinite(configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared) ||
|
|
configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared <= 0d ||
|
|
!IsFinite(configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared) ||
|
|
configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared <= 0d ||
|
|
!IsFinite(configuration.Longitudinal.MaximumJerkMetersPerSecondCubed) ||
|
|
configuration.Longitudinal.MaximumJerkMetersPerSecondCubed <= 0d ||
|
|
!IsFinite(configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond) ||
|
|
configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond <= 0d ||
|
|
!IsFinite(configuration.Validation.SpatialToleranceMeters) || configuration.Validation.SpatialToleranceMeters < 0d ||
|
|
!IsFinite(configuration.Validation.KinematicTolerance) || configuration.Validation.KinematicTolerance < 0d ||
|
|
!IsFinite(configuration.Corridor.MaximumCollisionCheckStepMeters) ||
|
|
configuration.Corridor.MaximumCollisionCheckStepMeters <= 0d)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
limits = new ValidationLimits(maximumSpeed, maximumCurvature,
|
|
configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared,
|
|
configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared,
|
|
configuration.Longitudinal.MaximumJerkMetersPerSecondCubed,
|
|
configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond,
|
|
configuration.Validation.SpatialToleranceMeters, configuration.Validation.KinematicTolerance,
|
|
configuration.Corridor.MaximumCollisionCheckStepMeters);
|
|
return true;
|
|
}
|
|
|
|
private static bool NearlyEqual(double expected, double actual, double tolerance)
|
|
{
|
|
double scale = Math.Max(1d, Math.Max(Math.Abs(expected), Math.Abs(actual)));
|
|
return Math.Abs(expected - actual) <= tolerance + tolerance * scale;
|
|
}
|
|
|
|
private static string Format(double value)
|
|
{
|
|
return value.ToString("G17", CultureInfo.InvariantCulture);
|
|
}
|
|
|
|
private static bool IsFinite(double value)
|
|
{
|
|
return !double.IsNaN(value) && !double.IsInfinity(value);
|
|
}
|
|
|
|
private static EmTrajectoryValidationResult Reject(EmTrajectoryValidationFailure failure, int index, string message)
|
|
{
|
|
return EmTrajectoryValidationResult.Reject(failure, index, message);
|
|
}
|
|
|
|
private readonly struct ValidationLimits
|
|
{
|
|
public ValidationLimits(double maximumSpeed, double maximumCurvature, double maximumAcceleration,
|
|
double maximumDeceleration, double maximumJerk, double maximumCurvatureRate, double spatialTolerance,
|
|
double kinematicTolerance, double configuredCollisionStepMeters)
|
|
{
|
|
MaximumSpeed = maximumSpeed;
|
|
MaximumCurvature = maximumCurvature;
|
|
MaximumAcceleration = maximumAcceleration;
|
|
MaximumDeceleration = maximumDeceleration;
|
|
MaximumJerk = maximumJerk;
|
|
MaximumCurvatureRate = maximumCurvatureRate;
|
|
SpatialTolerance = spatialTolerance;
|
|
KinematicTolerance = kinematicTolerance;
|
|
ConfiguredCollisionStepMeters = configuredCollisionStepMeters;
|
|
}
|
|
|
|
public double MaximumSpeed { get; }
|
|
public double MaximumCurvature { get; }
|
|
public double MaximumAcceleration { get; }
|
|
public double MaximumDeceleration { get; }
|
|
public double MaximumJerk { get; }
|
|
public double MaximumCurvatureRate { get; }
|
|
public double SpatialTolerance { get; }
|
|
public double KinematicTolerance { get; }
|
|
public double ConfiguredCollisionStepMeters { get; }
|
|
}
|
|
}
|