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

fix: accelerate EM trajectories from rest

Browse Source
This commit is contained in:
梁薄云
2026-08-07 07:51:43 +08:00
parent 162f1a24e6
commit 0bba8d7e61
9 changed files with 273 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs
+19 -1
View File
@@ -8,8 +8,15 @@ public sealed class LongitudinalSolutionValidator
{
public bool TryValidate(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate candidate,
out LongitudinalCandidate validatedCandidate, out string failureReason)
{
return TryValidate(input, speedLimit, candidate, out validatedCandidate, out _, out failureReason);
}
public bool TryValidate(LongitudinalPlanningInput input, PathSpeedLimit speedLimit, LongitudinalCandidate candidate,
out LongitudinalCandidate validatedCandidate, out EmPlanningStatus failureStatus, out string failureReason)
{
validatedCandidate = null;
failureStatus = EmPlanningStatus.LongitudinalInfeasible;
failureReason = string.Empty;
if (input == null || speedLimit == null || candidate == null)
{
@@ -58,7 +65,8 @@ public sealed class LongitudinalSolutionValidator
progress > input.PathUpperBoundS + tolerance || speed < -tolerance ||
acceleration < -maximumDeceleration - tolerance || acceleration > maximumAcceleration + tolerance)
{
failureReason = "ST candidate violates physical bounds at knot " + index + ".";
failureReason = "ST candidate violates physical bounds at knot " + index +
" (S=" + progress + ", U=" + speed + ", A=" + acceleration + ").";
return false;
}
double speedLimitAtProgress = index == 0
@@ -84,6 +92,16 @@ public sealed class LongitudinalSolutionValidator
return false;
}
}
bool requiresProgress = input.PathUpperBoundS >
input.Configuration.Validation.TerminalPositionToleranceMeters;
double achievedProgress = candidate.S[candidate.S.Count - 1] - candidate.S[0];
if (input.PlanningScope == EmPlanningScope.FullDirectionSegment && requiresProgress &&
achievedProgress <= input.Configuration.Validation.SpatialToleranceMeters)
{
failureStatus = EmPlanningStatus.NoProgress;
failureReason = "NoProgress: a nonterminal full direction segment produced zero progress.";
return false;
}
int stabilizationStart = candidate.S.Count;
if (input.Mode == EmLongitudinalMode.ExactStopAtBoundary)
{