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

feat: publish rolling trajectories without stop tails

Browse Source
This commit is contained in:
梁薄云
2026-08-05 20:00:11 +08:00
parent 72592f0cae
commit 4159ae0bc1
9 changed files with 196 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/ParkrobTrajplanner/EMPlanner/Trajectory/TrajectorySampleSchedule.cs
+26 -3
View File
@@ -8,7 +8,8 @@ internal sealed class TrajectorySampleSchedule
{
private const double ZeroTolerance = 1e-12d;
public TrajectorySampleSchedule(LongitudinalCandidate candidate, double outputTimeStepSeconds, double holdDurationSeconds)
public TrajectorySampleSchedule(LongitudinalCandidate candidate, double outputTimeStepSeconds, double holdDurationSeconds,
EmLongitudinalMode mode)
{
if (candidate == null)
throw new ArgumentNullException(nameof(candidate));
@@ -16,8 +17,8 @@ internal sealed class TrajectorySampleSchedule
throw new ArgumentOutOfRangeException(nameof(outputTimeStepSeconds));
if (!IsFinite(holdDurationSeconds) || holdDurationSeconds < 0d)
throw new ArgumentOutOfRangeException(nameof(holdDurationSeconds));
if (Math.Abs(candidate.U[candidate.U.Count - 1]) > ZeroTolerance)
throw new ArgumentException("A publishable trajectory requires an exact zero-speed terminal candidate.", nameof(candidate));
if (!Enum.IsDefined(typeof(EmLongitudinalMode), mode))
throw new ArgumentOutOfRangeException(nameof(mode));
var samples = new List<TrajectorySample>(candidate.KnotTimes.Count + 4);
double previousPathS = double.NegativeInfinity;
@@ -33,6 +34,27 @@ internal sealed class TrajectorySampleSchedule
previousPathS = candidate.S[index];
}
if (mode != EmLongitudinalMode.ExactStopAtBoundary)
{
Samples = new ReadOnlyCollection<TrajectorySample>(samples);
TerminalAnchorSampleIndex = -1;
return;
}
int stabilizationStart = LongitudinalTerminalSchedule.GetStabilizationStartIndex(candidate.KnotTimes,
outputTimeStepSeconds);
double stopPathS = candidate.S[stabilizationStart];
for (int index = stabilizationStart; index < candidate.S.Count; index++)
{
if (Math.Abs(candidate.S[index] - stopPathS) > ZeroTolerance ||
Math.Abs(candidate.U[index]) > ZeroTolerance || Math.Abs(candidate.A[index]) > ZeroTolerance ||
(index < candidate.J.Count && Math.Abs(candidate.J[index]) > ZeroTolerance))
{
throw new ArgumentException("An exact stop requires a stationary S/U/A/J tail.", nameof(candidate));
}
}
TerminalAnchorSampleIndex = stabilizationStart;
TrajectorySample terminal = samples[samples.Count - 1];
double holdElapsed = 0d;
while (holdElapsed < holdDurationSeconds - ZeroTolerance)
@@ -45,6 +67,7 @@ internal sealed class TrajectorySampleSchedule
}
public IReadOnlyList<TrajectorySample> Samples { get; }
public int TerminalAnchorSampleIndex { get; }
private static bool IsFinite(double value)
{