feat: validate EM planner requests
This commit is contained in:
梁薄云
@@ -0,0 +1,22 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
public sealed class CorridorConfiguration
|
||||
{
|
||||
public double LongitudinalSampleSpacingMeters { get; set; }
|
||||
public double LateralSampleSpacingMeters { get; set; }
|
||||
public double MaximumLateralOffsetMeters { get; set; }
|
||||
public double AdditionalClearanceReserveMeters { get; set; }
|
||||
public double MaximumCollisionCheckStepMeters { get; set; }
|
||||
|
||||
internal CorridorConfiguration Copy()
|
||||
{
|
||||
return new CorridorConfiguration
|
||||
{
|
||||
LongitudinalSampleSpacingMeters = LongitudinalSampleSpacingMeters,
|
||||
LateralSampleSpacingMeters = LateralSampleSpacingMeters,
|
||||
MaximumLateralOffsetMeters = MaximumLateralOffsetMeters,
|
||||
AdditionalClearanceReserveMeters = AdditionalClearanceReserveMeters,
|
||||
MaximumCollisionCheckStepMeters = MaximumCollisionCheckStepMeters,
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,111 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
public sealed partial class EmPlannerConfiguration
|
||||
{
|
||||
public SchedulingConfiguration Scheduling { get; set; }
|
||||
public FrenetConfiguration Frenet { get; set; }
|
||||
public CorridorConfiguration Corridor { get; set; }
|
||||
public LateralConfiguration Lateral { get; set; }
|
||||
public LongitudinalConfiguration Longitudinal { get; set; }
|
||||
public SolverConfiguration Solver { get; set; }
|
||||
public ValidationConfiguration Validation { get; set; }
|
||||
|
||||
public static EmPlannerConfiguration CreateDefault()
|
||||
{
|
||||
return new EmPlannerConfiguration
|
||||
{
|
||||
Scheduling = new SchedulingConfiguration
|
||||
{
|
||||
ReplanPeriodSeconds = 0.20d,
|
||||
TimeHorizonSeconds = 6d,
|
||||
DistanceHorizonMeters = 5d,
|
||||
OutputTimeStepSeconds = 0.05d,
|
||||
SolverTimeoutSeconds = 0.10d,
|
||||
HandoffLookaheadSeconds = 0.30d,
|
||||
MaximumVehicleStateAgeSeconds = 0.20d,
|
||||
},
|
||||
Corridor = new CorridorConfiguration
|
||||
{
|
||||
LongitudinalSampleSpacingMeters = 0.10d,
|
||||
LateralSampleSpacingMeters = 0.025d,
|
||||
MaximumLateralOffsetMeters = 0.30d,
|
||||
AdditionalClearanceReserveMeters = 0.02d,
|
||||
MaximumCollisionCheckStepMeters = 0.025d,
|
||||
},
|
||||
Frenet = new FrenetConfiguration
|
||||
{
|
||||
MaximumProjectionDistanceMeters = 0.50d,
|
||||
MinimumFrenetDenominator = 0.20d,
|
||||
BoundaryAnchorToleranceMeters = 1e-8d,
|
||||
},
|
||||
Lateral = new LateralConfiguration
|
||||
{
|
||||
MaximumLateralStepPerIterationMeters = 0.05d,
|
||||
MaximumLateralSlope = 0.50d,
|
||||
MaximumLateralSecondDerivativePerMeter = 1d,
|
||||
MaximumLateralThirdDerivativePerSquareMeter = 2d,
|
||||
Weights = new LateralWeights
|
||||
{
|
||||
ReferenceOffset = 10d,
|
||||
HeadingDeviation = 1d,
|
||||
SecondDerivative = 5d,
|
||||
ThirdDerivative = 10d,
|
||||
Curvature = 5d,
|
||||
CurvatureVariation = 20d,
|
||||
PreviousTrajectory = 5d,
|
||||
RollingTerminal = 10d,
|
||||
},
|
||||
},
|
||||
Longitudinal = new LongitudinalConfiguration
|
||||
{
|
||||
MaximumForwardSpeedMetersPerSecond = 0.20d,
|
||||
MaximumReverseSpeedMetersPerSecond = 0.20d,
|
||||
MaximumAccelerationMetersPerSecondSquared = 0.20d,
|
||||
MaximumDecelerationMetersPerSecondSquared = 0.30d,
|
||||
MaximumJerkMetersPerSecondCubed = 0.50d,
|
||||
MaximumLateralAccelerationMetersPerSecondSquared = 0.20d,
|
||||
MaximumCurvatureRatePerMeterPerSecond = 0.50d,
|
||||
StopSpeedToleranceMetersPerSecond = 0.01d,
|
||||
ZeroSpeedHoldSeconds = 0.20d,
|
||||
Weights = new LongitudinalWeights
|
||||
{
|
||||
ReferenceSpeed = 10d,
|
||||
Acceleration = 1d,
|
||||
Jerk = 10d,
|
||||
PreviousTrajectory = 5d,
|
||||
TerminalAcceleration = 1d,
|
||||
},
|
||||
},
|
||||
Solver = new SolverConfiguration
|
||||
{
|
||||
MaximumOuterIterations = 5,
|
||||
MaximumOsqpIterations = 4000,
|
||||
AbsoluteTolerance = 1e-5d,
|
||||
RelativeTolerance = 1e-5d,
|
||||
StrictResidualTolerance = 1e-5d,
|
||||
WarmStart = true,
|
||||
Polish = true,
|
||||
NativeVerbose = false,
|
||||
},
|
||||
Validation = new ValidationConfiguration
|
||||
{
|
||||
SpatialToleranceMeters = 1e-8d,
|
||||
KinematicTolerance = 1e-8d,
|
||||
},
|
||||
};
|
||||
}
|
||||
|
||||
internal EmPlannerConfiguration Copy()
|
||||
{
|
||||
return new EmPlannerConfiguration
|
||||
{
|
||||
Scheduling = Scheduling == null ? null : Scheduling.Copy(),
|
||||
Frenet = Frenet == null ? null : Frenet.Copy(),
|
||||
Corridor = Corridor == null ? null : Corridor.Copy(),
|
||||
Lateral = Lateral == null ? null : Lateral.Copy(),
|
||||
Longitudinal = Longitudinal == null ? null : Longitudinal.Copy(),
|
||||
Solver = Solver == null ? null : Solver.Copy(),
|
||||
Validation = Validation == null ? null : Validation.Copy(),
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,18 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
public sealed class FrenetConfiguration
|
||||
{
|
||||
public double MaximumProjectionDistanceMeters { get; set; }
|
||||
public double MinimumFrenetDenominator { get; set; }
|
||||
public double BoundaryAnchorToleranceMeters { get; set; }
|
||||
|
||||
internal FrenetConfiguration Copy()
|
||||
{
|
||||
return new FrenetConfiguration
|
||||
{
|
||||
MaximumProjectionDistanceMeters = MaximumProjectionDistanceMeters,
|
||||
MinimumFrenetDenominator = MinimumFrenetDenominator,
|
||||
BoundaryAnchorToleranceMeters = BoundaryAnchorToleranceMeters,
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,22 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
public sealed class LateralConfiguration
|
||||
{
|
||||
public double MaximumLateralStepPerIterationMeters { get; set; }
|
||||
public double MaximumLateralSlope { get; set; }
|
||||
public double MaximumLateralSecondDerivativePerMeter { get; set; }
|
||||
public double MaximumLateralThirdDerivativePerSquareMeter { get; set; }
|
||||
public LateralWeights Weights { get; set; }
|
||||
|
||||
internal LateralConfiguration Copy()
|
||||
{
|
||||
return new LateralConfiguration
|
||||
{
|
||||
MaximumLateralStepPerIterationMeters = MaximumLateralStepPerIterationMeters,
|
||||
MaximumLateralSlope = MaximumLateralSlope,
|
||||
MaximumLateralSecondDerivativePerMeter = MaximumLateralSecondDerivativePerMeter,
|
||||
MaximumLateralThirdDerivativePerSquareMeter = MaximumLateralThirdDerivativePerSquareMeter,
|
||||
Weights = Weights == null ? null : Weights.Copy(),
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,28 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
public sealed class LateralWeights
|
||||
{
|
||||
public double ReferenceOffset { get; set; }
|
||||
public double HeadingDeviation { get; set; }
|
||||
public double SecondDerivative { get; set; }
|
||||
public double ThirdDerivative { get; set; }
|
||||
public double Curvature { get; set; }
|
||||
public double CurvatureVariation { get; set; }
|
||||
public double PreviousTrajectory { get; set; }
|
||||
public double RollingTerminal { get; set; }
|
||||
|
||||
internal LateralWeights Copy()
|
||||
{
|
||||
return new LateralWeights
|
||||
{
|
||||
ReferenceOffset = ReferenceOffset,
|
||||
HeadingDeviation = HeadingDeviation,
|
||||
SecondDerivative = SecondDerivative,
|
||||
ThirdDerivative = ThirdDerivative,
|
||||
Curvature = Curvature,
|
||||
CurvatureVariation = CurvatureVariation,
|
||||
PreviousTrajectory = PreviousTrajectory,
|
||||
RollingTerminal = RollingTerminal,
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,32 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
public sealed class LongitudinalConfiguration
|
||||
{
|
||||
public double MaximumForwardSpeedMetersPerSecond { get; set; }
|
||||
public double MaximumReverseSpeedMetersPerSecond { get; set; }
|
||||
public double MaximumAccelerationMetersPerSecondSquared { get; set; }
|
||||
public double MaximumDecelerationMetersPerSecondSquared { get; set; }
|
||||
public double MaximumJerkMetersPerSecondCubed { get; set; }
|
||||
public double MaximumLateralAccelerationMetersPerSecondSquared { get; set; }
|
||||
public double MaximumCurvatureRatePerMeterPerSecond { get; set; }
|
||||
public double StopSpeedToleranceMetersPerSecond { get; set; }
|
||||
public double ZeroSpeedHoldSeconds { get; set; }
|
||||
public LongitudinalWeights Weights { get; set; }
|
||||
|
||||
internal LongitudinalConfiguration Copy()
|
||||
{
|
||||
return new LongitudinalConfiguration
|
||||
{
|
||||
MaximumForwardSpeedMetersPerSecond = MaximumForwardSpeedMetersPerSecond,
|
||||
MaximumReverseSpeedMetersPerSecond = MaximumReverseSpeedMetersPerSecond,
|
||||
MaximumAccelerationMetersPerSecondSquared = MaximumAccelerationMetersPerSecondSquared,
|
||||
MaximumDecelerationMetersPerSecondSquared = MaximumDecelerationMetersPerSecondSquared,
|
||||
MaximumJerkMetersPerSecondCubed = MaximumJerkMetersPerSecondCubed,
|
||||
MaximumLateralAccelerationMetersPerSecondSquared = MaximumLateralAccelerationMetersPerSecondSquared,
|
||||
MaximumCurvatureRatePerMeterPerSecond = MaximumCurvatureRatePerMeterPerSecond,
|
||||
StopSpeedToleranceMetersPerSecond = StopSpeedToleranceMetersPerSecond,
|
||||
ZeroSpeedHoldSeconds = ZeroSpeedHoldSeconds,
|
||||
Weights = Weights == null ? null : Weights.Copy(),
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,22 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
public sealed class LongitudinalWeights
|
||||
{
|
||||
public double ReferenceSpeed { get; set; }
|
||||
public double Acceleration { get; set; }
|
||||
public double Jerk { get; set; }
|
||||
public double PreviousTrajectory { get; set; }
|
||||
public double TerminalAcceleration { get; set; }
|
||||
|
||||
internal LongitudinalWeights Copy()
|
||||
{
|
||||
return new LongitudinalWeights
|
||||
{
|
||||
ReferenceSpeed = ReferenceSpeed,
|
||||
Acceleration = Acceleration,
|
||||
Jerk = Jerk,
|
||||
PreviousTrajectory = PreviousTrajectory,
|
||||
TerminalAcceleration = TerminalAcceleration,
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,26 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
public sealed class SchedulingConfiguration
|
||||
{
|
||||
public double ReplanPeriodSeconds { get; set; }
|
||||
public double TimeHorizonSeconds { get; set; }
|
||||
public double DistanceHorizonMeters { get; set; }
|
||||
public double OutputTimeStepSeconds { get; set; }
|
||||
public double SolverTimeoutSeconds { get; set; }
|
||||
public double HandoffLookaheadSeconds { get; set; }
|
||||
public double MaximumVehicleStateAgeSeconds { get; set; }
|
||||
|
||||
internal SchedulingConfiguration Copy()
|
||||
{
|
||||
return new SchedulingConfiguration
|
||||
{
|
||||
ReplanPeriodSeconds = ReplanPeriodSeconds,
|
||||
TimeHorizonSeconds = TimeHorizonSeconds,
|
||||
DistanceHorizonMeters = DistanceHorizonMeters,
|
||||
OutputTimeStepSeconds = OutputTimeStepSeconds,
|
||||
SolverTimeoutSeconds = SolverTimeoutSeconds,
|
||||
HandoffLookaheadSeconds = HandoffLookaheadSeconds,
|
||||
MaximumVehicleStateAgeSeconds = MaximumVehicleStateAgeSeconds,
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,28 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
public sealed class SolverConfiguration
|
||||
{
|
||||
public int MaximumOuterIterations { get; set; }
|
||||
public int MaximumOsqpIterations { get; set; }
|
||||
public double AbsoluteTolerance { get; set; }
|
||||
public double RelativeTolerance { get; set; }
|
||||
public double StrictResidualTolerance { get; set; }
|
||||
public bool WarmStart { get; set; }
|
||||
public bool Polish { get; set; }
|
||||
public bool NativeVerbose { get; set; }
|
||||
|
||||
internal SolverConfiguration Copy()
|
||||
{
|
||||
return new SolverConfiguration
|
||||
{
|
||||
MaximumOuterIterations = MaximumOuterIterations,
|
||||
MaximumOsqpIterations = MaximumOsqpIterations,
|
||||
AbsoluteTolerance = AbsoluteTolerance,
|
||||
RelativeTolerance = RelativeTolerance,
|
||||
StrictResidualTolerance = StrictResidualTolerance,
|
||||
WarmStart = WarmStart,
|
||||
Polish = Polish,
|
||||
NativeVerbose = NativeVerbose,
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,16 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
public sealed class ValidationConfiguration
|
||||
{
|
||||
public double SpatialToleranceMeters { get; set; }
|
||||
public double KinematicTolerance { get; set; }
|
||||
|
||||
internal ValidationConfiguration Copy()
|
||||
{
|
||||
return new ValidationConfiguration
|
||||
{
|
||||
SpatialToleranceMeters = SpatialToleranceMeters,
|
||||
KinematicTolerance = KinematicTolerance,
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -12,17 +12,6 @@ public sealed class VehicleMotionState
|
||||
DateTimeOffset capturedAtUtc,
|
||||
long sequenceId)
|
||||
{
|
||||
if (pose == null)
|
||||
throw new ArgumentNullException(nameof(pose));
|
||||
ContractNumeric.RequireFinite(pose.X, nameof(pose));
|
||||
ContractNumeric.RequireFinite(pose.Y, nameof(pose));
|
||||
ContractNumeric.RequireFinite(pose.Heading, nameof(pose));
|
||||
ContractNumeric.RequireFinite(signedLongitudinalSpeedMetersPerSecond, nameof(signedLongitudinalSpeedMetersPerSecond));
|
||||
if (longitudinalAccelerationMetersPerSecondSquared.HasValue)
|
||||
ContractNumeric.RequireFinite(longitudinalAccelerationMetersPerSecondSquared.Value, nameof(longitudinalAccelerationMetersPerSecondSquared));
|
||||
if (sequenceId < 0)
|
||||
throw new ArgumentOutOfRangeException(nameof(sequenceId));
|
||||
|
||||
Pose = pose;
|
||||
SignedLongitudinalSpeedMetersPerSecond = signedLongitudinalSpeedMetersPerSecond;
|
||||
LongitudinalAccelerationMetersPerSecondSquared = longitudinalAccelerationMetersPerSecondSquared;
|
||||
|
||||
@@ -0,0 +1,13 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
public sealed class EmPlannerDebugOptions
|
||||
{
|
||||
public bool EnableSummary { get; set; }
|
||||
public bool EnableProjectionTrace { get; set; }
|
||||
public bool EnableCorridorTrace { get; set; }
|
||||
public bool EnableLateralSolverTrace { get; set; }
|
||||
public bool EnableLongitudinalSolverTrace { get; set; }
|
||||
public bool EnableTrajectoryDump { get; set; }
|
||||
public bool EnableVisualization { get; set; }
|
||||
public IEmPlannerDebugSink Sink { get; set; }
|
||||
}
|
||||
@@ -0,0 +1,30 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
public sealed class EmPlanningDiagnostics
|
||||
{
|
||||
private readonly List<string> _debugSinkFailures = new List<string>();
|
||||
|
||||
public IReadOnlyList<string> DebugSinkFailures
|
||||
{
|
||||
get { return new ReadOnlyCollection<string>(new List<string>(_debugSinkFailures)); }
|
||||
}
|
||||
|
||||
public void WriteDebug(EmPlannerDebugOptions options, string message)
|
||||
{
|
||||
if (options == null || options.Sink == null)
|
||||
return;
|
||||
|
||||
try
|
||||
{
|
||||
options.Sink.Write(message ?? string.Empty);
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
_debugSinkFailures.Add(exception.GetType().FullName + ": " + exception.Message);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,6 @@
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
public interface IEmPlannerDebugSink
|
||||
{
|
||||
void Write(string message);
|
||||
}
|
||||
@@ -0,0 +1,165 @@
|
||||
using System;
|
||||
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 (!TryValidateConfiguration(request.Configuration, 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, 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.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) ||
|
||||
!WeightsAreValid(configuration.Lateral.Weights) || !WeightsAreValid(configuration.Longitudinal.Weights))
|
||||
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);
|
||||
}
|
||||
}
|
||||
@@ -1,11 +1,20 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.TrajectoryPlanning.CoarsePath;
|
||||
using MultiWheelC.TrajectoryPlanning.Mapping;
|
||||
using MultiWheelC.TrajectoryPlanning.PathSmoothing;
|
||||
|
||||
namespace MultiWheelC.TrajectoryPlanning.EMPlanner;
|
||||
|
||||
internal static class FoundationChecks
|
||||
{
|
||||
public static void Run()
|
||||
{
|
||||
VerifyContracts();
|
||||
VerifyConfigurationAndRequestValidation();
|
||||
}
|
||||
|
||||
private static void VerifyContracts()
|
||||
{
|
||||
var reverseState = new VehicleMotionState(
|
||||
new Pose2D(1.5d, -2d, 0.25d),
|
||||
@@ -48,4 +57,190 @@ internal static class FoundationChecks
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(-0.12d * Math.Sin(0.5d), point.VelocityY, "point derived velocity y");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual((-0.12d) * (-0.2d), point.YawRate, "point derived yaw rate");
|
||||
}
|
||||
|
||||
private static void VerifyConfigurationAndRequestValidation()
|
||||
{
|
||||
EmPlannerConfiguration configuration = EmPlannerConfiguration.CreateDefault();
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Scheduling.ReplanPeriodSeconds, "replan period");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(6d, configuration.Scheduling.TimeHorizonSeconds, "time horizon");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(5d, configuration.Scheduling.DistanceHorizonMeters, "distance horizon");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.05d, configuration.Scheduling.OutputTimeStepSeconds, "output time step");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.10d, configuration.Scheduling.SolverTimeoutSeconds, "solver timeout");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.30d, configuration.Scheduling.HandoffLookaheadSeconds, "handoff lookahead");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Scheduling.MaximumVehicleStateAgeSeconds, "vehicle state age");
|
||||
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.10d, configuration.Corridor.LongitudinalSampleSpacingMeters, "longitudinal sample spacing");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.025d, configuration.Corridor.LateralSampleSpacingMeters, "lateral sample spacing");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.30d, configuration.Corridor.MaximumLateralOffsetMeters, "maximum lateral offset");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.02d, configuration.Corridor.AdditionalClearanceReserveMeters, "clearance reserve");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.025d, configuration.Corridor.MaximumCollisionCheckStepMeters, "collision check step");
|
||||
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.50d, configuration.Frenet.MaximumProjectionDistanceMeters, "maximum projection distance");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Frenet.MinimumFrenetDenominator, "minimum Frenet denominator");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(1e-8d, configuration.Frenet.BoundaryAnchorToleranceMeters, "boundary anchor tolerance");
|
||||
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.05d, configuration.Lateral.MaximumLateralStepPerIterationMeters, "lateral trust region");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.50d, configuration.Lateral.MaximumLateralSlope, "maximum lateral slope");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(1d, configuration.Lateral.MaximumLateralSecondDerivativePerMeter, "maximum lateral second derivative");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(2d, configuration.Lateral.MaximumLateralThirdDerivativePerSquareMeter, "maximum lateral third derivative");
|
||||
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Longitudinal.MaximumForwardSpeedMetersPerSecond, "maximum forward speed");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Longitudinal.MaximumReverseSpeedMetersPerSecond, "maximum reverse speed");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Longitudinal.MaximumAccelerationMetersPerSecondSquared, "maximum acceleration");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.30d, configuration.Longitudinal.MaximumDecelerationMetersPerSecondSquared, "maximum deceleration");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.50d, configuration.Longitudinal.MaximumJerkMetersPerSecondCubed, "maximum jerk");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Longitudinal.MaximumLateralAccelerationMetersPerSecondSquared, "maximum lateral acceleration");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.50d, configuration.Longitudinal.MaximumCurvatureRatePerMeterPerSecond, "maximum curvature rate");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.01d, configuration.Longitudinal.StopSpeedToleranceMetersPerSecond, "stop speed tolerance");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(0.20d, configuration.Longitudinal.ZeroSpeedHoldSeconds, "zero speed hold");
|
||||
|
||||
EMPlannerVerificationHost.Verification.Equal(5, configuration.Solver.MaximumOuterIterations, "maximum outer iterations");
|
||||
EMPlannerVerificationHost.Verification.Equal(4000, configuration.Solver.MaximumOsqpIterations, "maximum OSQP iterations");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(1e-5d, configuration.Solver.AbsoluteTolerance, "absolute tolerance");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(1e-5d, configuration.Solver.RelativeTolerance, "relative tolerance");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(1e-5d, configuration.Solver.StrictResidualTolerance, "strict residual tolerance");
|
||||
EMPlannerVerificationHost.Verification.Equal(true, configuration.Solver.WarmStart, "warm start");
|
||||
EMPlannerVerificationHost.Verification.Equal(true, configuration.Solver.Polish, "polish");
|
||||
EMPlannerVerificationHost.Verification.Equal(false, configuration.Solver.NativeVerbose, "native verbose");
|
||||
|
||||
VerifyLateralWeights(configuration.Lateral.Weights);
|
||||
VerifyLongitudinalWeights(configuration.Longitudinal.Weights);
|
||||
|
||||
EmPlanningRequest valid = CreateValidRequest(configuration);
|
||||
AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate(
|
||||
CreateRequest(null!, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "null reference path");
|
||||
AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate(
|
||||
CreateRequest(valid.ReferencePath, null!, valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "null map");
|
||||
AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate(
|
||||
CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, valid.VehicleState, null!, 0, EmMotionModel.NonholonomicForwardReverse)), "null configuration");
|
||||
AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate(
|
||||
CreateRequest(valid.ReferencePath, CreateMap(false), valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "map not ready");
|
||||
AssertStatus(EmPlanningStatus.InvalidReferencePath, EmPlanningRequestValidator.Validate(
|
||||
CreateRequest(CreateFailedReferencePath(), valid.Map, valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "smoothing failure");
|
||||
AssertStatus(EmPlanningStatus.InvalidReferencePath, EmPlanningRequestValidator.Validate(
|
||||
CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 1, EmMotionModel.NonholonomicForwardReverse)), "segment index");
|
||||
AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate(
|
||||
CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, CreateState(double.NaN, 9L, valid.RequestedAtUtc), configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "non-finite speed");
|
||||
AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate(
|
||||
CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, CreateState(0d, -1L, valid.RequestedAtUtc), configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "negative sequence ID");
|
||||
AssertStatus(EmPlanningStatus.StaleVehicleState, EmPlanningRequestValidator.Validate(
|
||||
CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, CreateState(0d, 9L, valid.RequestedAtUtc.AddSeconds(-1d)), configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "stale state");
|
||||
AssertStatus(EmPlanningStatus.InvalidInput, EmPlanningRequestValidator.Validate(
|
||||
CreateRequest(valid.ReferencePath, valid.Map, new VehicleParameters { LengthMeters = 1d, WidthMeters = 0.5d }, valid.VehicleState, configuration, 0, EmMotionModel.NonholonomicForwardReverse)), "missing curvature limit");
|
||||
AssertStatus(EmPlanningStatus.UnsupportedMotionMode, EmPlanningRequestValidator.Validate(
|
||||
CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.CrabTranslation)), "crab motion");
|
||||
AssertStatus(EmPlanningStatus.UnsupportedMotionMode, EmPlanningRequestValidator.Validate(
|
||||
CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 0, EmMotionModel.InPlaceRotation)), "in-place rotation");
|
||||
|
||||
EmPlanningRequestValidationResult first = EmPlanningRequestValidator.Validate(
|
||||
CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 1, EmMotionModel.NonholonomicForwardReverse));
|
||||
EmPlanningRequestValidationResult second = EmPlanningRequestValidator.Validate(
|
||||
CreateRequest(valid.ReferencePath, valid.Map, valid.Vehicle, valid.VehicleState, configuration, 1, EmMotionModel.NonholonomicForwardReverse));
|
||||
EMPlannerVerificationHost.Verification.Equal(first.FailureReason, second.FailureReason, "deterministic rejection message");
|
||||
}
|
||||
|
||||
private static void VerifyLateralWeights(LateralWeights weights)
|
||||
{
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(10d, weights.ReferenceOffset, "lateral reference weight");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(1d, weights.HeadingDeviation, "lateral heading weight");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(5d, weights.SecondDerivative, "lateral second derivative weight");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(10d, weights.ThirdDerivative, "lateral third derivative weight");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(5d, weights.Curvature, "lateral curvature weight");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(20d, weights.CurvatureVariation, "lateral curvature variation weight");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(5d, weights.PreviousTrajectory, "lateral previous weight");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(10d, weights.RollingTerminal, "lateral rolling terminal weight");
|
||||
}
|
||||
|
||||
private static void VerifyLongitudinalWeights(LongitudinalWeights weights)
|
||||
{
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(10d, weights.ReferenceSpeed, "longitudinal speed weight");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(1d, weights.Acceleration, "longitudinal acceleration weight");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(10d, weights.Jerk, "longitudinal jerk weight");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(5d, weights.PreviousTrajectory, "longitudinal previous weight");
|
||||
EMPlannerVerificationHost.Verification.NearlyEqual(1d, weights.TerminalAcceleration, "longitudinal terminal acceleration weight");
|
||||
}
|
||||
|
||||
private static void AssertStatus(EmPlanningStatus expected, EmPlanningRequestValidationResult actual, string name)
|
||||
{
|
||||
EMPlannerVerificationHost.Verification.Equal(expected, actual.Status, name);
|
||||
}
|
||||
|
||||
private static EmPlanningRequest CreateValidRequest(EmPlannerConfiguration configuration)
|
||||
{
|
||||
DateTimeOffset requestedAtUtc = new DateTimeOffset(2026, 8, 3, 1, 0, 0, TimeSpan.Zero);
|
||||
return CreateRequest(
|
||||
CreateValidReferencePath(),
|
||||
CreateMap(true),
|
||||
new VehicleParameters { LengthMeters = 1d, WidthMeters = 0.5d, SafetyMarginMeters = 0.05d, MaximumCurvaturePerMeter = 0.5d },
|
||||
CreateState(0d, 9L, requestedAtUtc),
|
||||
configuration,
|
||||
0,
|
||||
EmMotionModel.NonholonomicForwardReverse);
|
||||
}
|
||||
|
||||
private static EmPlanningRequest CreateRequest(
|
||||
PathSmoothingResult referencePath,
|
||||
PlanningGridMap map,
|
||||
VehicleParameters vehicle,
|
||||
VehicleMotionState vehicleState,
|
||||
EmPlannerConfiguration configuration,
|
||||
int segmentIndex,
|
||||
EmMotionModel motionModel)
|
||||
{
|
||||
DateTimeOffset requestedAtUtc = new DateTimeOffset(2026, 8, 3, 1, 0, 0, TimeSpan.Zero);
|
||||
return new EmPlanningRequest(
|
||||
referencePath,
|
||||
map,
|
||||
vehicle,
|
||||
vehicleState,
|
||||
configuration,
|
||||
segmentIndex,
|
||||
null!,
|
||||
requestedAtUtc,
|
||||
requestedAtUtc.AddSeconds(0.1d),
|
||||
"trajectory-1",
|
||||
"reference-1",
|
||||
string.Empty,
|
||||
motionModel);
|
||||
}
|
||||
|
||||
private static VehicleMotionState CreateState(double speed, long sequenceId, DateTimeOffset capturedAtUtc)
|
||||
{
|
||||
return new VehicleMotionState(new Pose2D(0d, 0d, 0d), speed, null, capturedAtUtc, sequenceId);
|
||||
}
|
||||
|
||||
private static PlanningGridMap CreateMap(bool planningReady)
|
||||
{
|
||||
var result = new PlanningMapFactory().Create(new PlanningMapRequest
|
||||
{
|
||||
Bounds = new MapBoundsMm(-2000f, 2000f, -2000f, 2000f),
|
||||
ResolutionMm = 100f,
|
||||
ObstacleSources = new List<IMapObstacleSource>(),
|
||||
AllowExplicitEmptyMap = planningReady,
|
||||
});
|
||||
if (!result.Succeeded || result.Map == null)
|
||||
throw new InvalidOperationException("Unable to create test planning map.");
|
||||
return result.Map;
|
||||
}
|
||||
|
||||
private static PathSmoothingResult CreateValidReferencePath()
|
||||
{
|
||||
var points = new List<SmoothedPathPoint>
|
||||
{
|
||||
new SmoothedPathPoint(0d, 0d, 0d, 0d, 0d, TravelDirection.Forward, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
|
||||
new SmoothedPathPoint(1d, 0d, 0d, 0d, 1d, TravelDirection.Forward, 0d, 0d, 1d, false, SmoothedPathPointSource.Anchor),
|
||||
};
|
||||
var segments = new List<SmoothedPathSegment>
|
||||
{
|
||||
new SmoothedPathSegment(0, TravelDirection.Forward, 0, 1, false, false),
|
||||
};
|
||||
var metrics = new PathQualityMetrics(true, 1d, 0d, 0d, 0d, 0d, 1d, 0d, 0d, 0d, 0d, 0d);
|
||||
return PathSmoothingResult.PublishLocalG2(PathSmoothingStatus.Complete, points, segments,
|
||||
new PathSmoothingDiagnostics(metrics, TimeSpan.Zero), new List<PathSmoothingRegionReport>());
|
||||
}
|
||||
|
||||
private static PathSmoothingResult CreateFailedReferencePath()
|
||||
{
|
||||
return PathSmoothingResult.Failure(PathSmoothingStatus.Failed, new PathSmoothingDiagnostics());
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user