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

feat: reuse prior trajectory in longitudinal planning

Browse Source
This commit is contained in:
梁薄云
2026-08-05 20:32:40 +08:00
parent 4159ae0bc1
commit 6cfbaf61f5
4 changed files with 413 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/tests/EMPlannerVerificationHost/LongitudinalModelChecks.cs
+51
View File
@@ -20,6 +20,7 @@ internal static class LongitudinalModelChecks
VerifiesReferenceHorizonSelectionSeparatesSpaceAndTime();
VerifiesTimeKnotLayoutDynamicsObjectiveAndHardConstraints();
VerifiesModeSpecificSolutionValidation();
VerifiesPreviousTrajectorySeedResamplesAndProjectsMonotonically();
}
private static void VerifiesJerkLimitedStoppingProfileEndsAtRest()
@@ -454,6 +455,34 @@ internal static class LongitudinalModelChecks
"approach failure identifies the jerk-limited stoppable set");
}
private static void VerifiesPreviousTrajectorySeedResamplesAndProjectsMonotonically()
{
LateralPath path = CreateStraightPath(1d);
DateTimeOffset previousEffectiveAtUtc = DateTimeOffset.UnixEpoch.AddSeconds(10d);
EmTrajectory previous = CreatePreviousTrajectory(previousEffectiveAtUtc, TravelDirection.Forward, 3);
var builder = new LongitudinalPreviousTrajectorySeedBuilder();
LongitudinalPreviousTrajectorySeed seed = builder.Build(previous, path, previousEffectiveAtUtc.AddSeconds(0.20d),
new[] { 0d, 0.10d, 0.20d }, 3, TravelDirection.Forward);
Verification.Equal(3, seed.PathS.Count, "previous seed path-S count");
Verification.Equal(3, seed.ProgressSpeedMetersPerSecond.Count, "previous seed speed count");
Verification.NearlyEqual(0.20d, seed.PathS[0], "previous seed begins at new absolute effective time");
Verification.True(seed.PathS[1] >= seed.PathS[0] && seed.PathS[2] >= seed.PathS[1],
"previous seed progress is monotone");
Verification.NearlyEqual(0.10d, seed.ProgressSpeedMetersPerSecond[0],
"previous seed uses absolute progress speed");
Verification.Equal(0, builder.Build(previous, path, previousEffectiveAtUtc.AddSeconds(0.20d),
new[] { 0d, 0.10d, 0.20d }, 3, TravelDirection.Reverse).PathS.Count,
"different direction returns an empty seed");
Verification.Equal(0, builder.Build(previous, path, previousEffectiveAtUtc.AddSeconds(0.20d),
new[] { 0d, 0.10d, 0.20d }, 4, TravelDirection.Forward).PathS.Count,
"different segment returns an empty seed");
Verification.Equal(0, builder.Build(previous, path, previousEffectiveAtUtc.AddSeconds(0.40d),
new[] { 0d, 0.10d }, 3, TravelDirection.Forward).PathS.Count,
"out-of-range absolute sampling returns an empty seed");
}
private static LateralPath CreatePath(IReadOnlyList<PathFixture> fixtures)
{
var points = new List<LateralPathPoint>(fixtures.Count);
@@ -466,6 +495,28 @@ internal static class LongitudinalModelChecks
return new LateralPath(points, true);
}
private static EmTrajectory CreatePreviousTrajectory(DateTimeOffset effectiveAtUtc, TravelDirection direction,
int segmentIndex)
{
var metadata = new EmTrajectoryMetadata("previous-seed", effectiveAtUtc, effectiveAtUtc, 1L,
"previous-reference", 1L, string.Empty, segmentIndex, direction, EmTerminalType.RollingSafetyStop,
EmLongitudinalMode.RollingContinuation);
double sign = direction == TravelDirection.Forward ? 1d : -1d;
return new EmTrajectory(metadata, new[]
{
new EmTrajectoryPoint(0d, 0d, 0d, sign * 0.10d, 0d, 0d, segmentIndex, 0d, 0d,
direction, EmBoundaryType.None, 0d, 0d),
new EmTrajectoryPoint(sign * 0.10d, 0d, 0d, sign * 0.10d, 0.10d, 0d, segmentIndex, 0.10d, 0.10d,
direction, EmBoundaryType.None, 0d, 0d),
new EmTrajectoryPoint(sign * 0.20d, 0d, 0d, sign * 0.10d, 0.20d, 0d, segmentIndex, 0.20d, 0.20d,
direction, EmBoundaryType.None, 0d, 0d),
new EmTrajectoryPoint(sign * 0.30d, 0d, 0d, sign * 0.10d, 0.30d, 0d, segmentIndex, 0.30d, 0.30d,
direction, EmBoundaryType.None, 0d, 0d),
new EmTrajectoryPoint(sign * 0.40d, 0d, 0d, sign * 0.10d, 0.40d, 0d, segmentIndex, 0.40d, 0.40d,
direction, EmBoundaryType.None, 0d, 0d),
});
}
private static DirectionSegmentView CreateSegment(double length, EmBoundaryType endBoundaryType)
{
var points = new List<SmoothedPathPoint>