First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

feat: define full-direction EM planning scope

Browse Source
This commit is contained in:
梁薄云
2026-08-06 20:48:44 +08:00
parent fcf7df17d5
commit f4e89b4b4f
20 changed files with 183 additions and 33 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Validation/EmPlanningRequestValidator.cs
+16 -3
View File
@@ -49,7 +49,9 @@ public static class EmPlanningRequestValidator
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))
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.");
@@ -109,7 +111,8 @@ public static class EmPlanningRequestValidator
VehicleKinematics.TryGetMaximumCurvaturePerMeter(vehicle, out _);
}
private static bool TryValidateConfiguration(EmPlannerConfiguration configuration, out string failureReason)
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 ||
@@ -134,12 +137,21 @@ public static class EmPlanningRequestValidator
!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;
@@ -162,7 +174,8 @@ public static class EmPlanningRequestValidator
longitudinal.MaximumReverseSpeedMetersPerSecond);
double requiredDistanceHorizon = Math.Max(forwardStop.DistanceMeters, reverseStop.DistanceMeters) +
maximumSpeed * scheduling.ReplanPeriodSeconds;
if (scheduling.DistanceHorizonMeters + 1e-12d < requiredDistanceHorizon)
if (planningScope == EmPlanningScope.RollingHorizon &&
scheduling.DistanceHorizonMeters + 1e-12d < requiredDistanceHorizon)
{
failureReason = "DistanceHorizonMeters is insufficient: required=" + requiredDistanceHorizon +
";configured=" + scheduling.DistanceHorizonMeters;