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

feat: derive adaptive full-segment ST schedule

Browse Source
This commit is contained in:
梁薄云
2026-08-06 23:54:34 +08:00
parent dad4ff4b04
commit 3269d556b6
14 changed files with 1642 additions and 108 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Longitudinal/LongitudinalSolutionValidator.cs
+18 -5
View File
@@ -23,12 +23,11 @@ public sealed class LongitudinalSolutionValidator
{
return false;
}
IReadOnlyList<double> expectedTimes = LongitudinalCandidate.CreateKnotTimes(
input.Configuration.Scheduling.TimeHorizonSeconds, input.Configuration.Scheduling.OutputTimeStepSeconds);
IReadOnlyList<double> expectedTimes = input.KnotSchedule.KnotTimes;
double tolerance = RequireNonnegative(input.Configuration.Validation.KinematicTolerance, nameof(tolerance));
if (!HasMatchingTimes(candidate.KnotTimes, expectedTimes, tolerance))
{
failureReason = "ST candidate knot times do not match the configured horizon.";
failureReason = "ST candidate knot times do not match the supplied knot schedule.";
return false;
}
if (candidate.S.Count != expectedTimes.Count || candidate.U.Count != expectedTimes.Count ||
@@ -88,8 +87,7 @@ public sealed class LongitudinalSolutionValidator
int stabilizationStart = candidate.S.Count;
if (input.Mode == EmLongitudinalMode.ExactStopAtBoundary)
{
stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(
candidate.KnotTimes, input.Configuration.Scheduling.OutputTimeStepSeconds);
stabilizationStart = GetStabilizationStart(input, candidate.KnotTimes);
for (int index = stabilizationStart; index < candidate.S.Count; index++)
{
if (!AreClose(candidate.S[index], input.StopBoundaryPathS, tolerance) ||
@@ -166,6 +164,21 @@ public sealed class LongitudinalSolutionValidator
return true;
}
private static int GetStabilizationStart(LongitudinalPlanningInput input, IReadOnlyList<double> times)
{
if (input.PlanningScope == EmPlanningScope.FullDirectionSegment)
{
if (input.KnotSchedule.TerminalHoldStartIndex < 1 ||
input.KnotSchedule.TerminalHoldStartIndex >= times.Count)
{
throw new ArgumentException("Full-direction exact-stop schedules require an explicit terminal hold boundary.");
}
return input.KnotSchedule.TerminalHoldStartIndex;
}
return LongitudinalTerminalSchedule.GetStabilizationStartIndex(times,
input.Configuration.Scheduling.OutputTimeStepSeconds);
}
private static bool AreClose(double actual, double expected, double tolerance)
{
return Math.Abs(actual - expected) <= tolerance;