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

test: verify rolling EM execution

Browse Source
This commit is contained in:
梁薄云
2026-08-04 13:28:13 +08:00
parent 3c84e36893
commit 4058230eb8
6 changed files with 322 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/tests/EMPlannerVerificationHost/ExecutorChecks.cs
+39
View File
@@ -15,6 +15,15 @@ internal static class ExecutorChecks
VerifiesVehicleStateProviderRemainsSnapshotOnly();
}
public static void RunRollingEndToEnd()
{
VerifiesGearSwitchSequence(TravelDirection.Forward, TravelDirection.Reverse,
"end-to-end forward-to-reverse");
VerifiesGearSwitchSequence(TravelDirection.Reverse, TravelDirection.Forward,
"end-to-end reverse-to-forward");
VerifiesGoalCommandStopsAtAndAfterTerminal();
}
private static void VerifiesGearSwitchSequence(TravelDirection currentDirection, TravelDirection desiredDirection,
string name)
{
@@ -191,6 +200,36 @@ internal static class ExecutorChecks
Verification.Equal(84L, provider.Capture().SequenceId, "vehicle state provider returns caller-owned snapshot");
}
private static void VerifiesGoalCommandStopsAtAndAfterTerminal()
{
DateTimeOffset effectiveAt = DateTimeOffset.UnixEpoch.AddSeconds(950d);
EmTrajectory goalTrajectory = CreateMotionTrajectory(effectiveAt, TravelDirection.Forward, 0.10d, 0.20d);
var executor = new TrajectoryExecutor();
VehicleMotionState moving = CreateMeasuredState(0.10d, effectiveAt, 95L);
TrajectoryControlCommand following = executor.UpdateCommand(effectiveAt, moving, goalTrajectory,
TravelDirection.Forward, TravelDirection.Forward, false);
Verification.NearlyEqual(goalTrajectory.Points[0].SignedLongitudinalVelocity, following.SignedLongitudinalVelocity,
"goal approach command originates from trajectory point");
Verification.NearlyEqual(goalTrajectory.Points[0].YawRate, following.YawRate,
"goal approach command preserves trajectory yaw rate");
TrajectoryControlCommand atGoal = executor.UpdateCommand(effectiveAt.AddSeconds(0.30d),
CreateMeasuredState(0d, effectiveAt.AddSeconds(0.30d), 96L), goalTrajectory, TravelDirection.Forward,
TravelDirection.Forward, false);
TrajectoryControlCommand afterGoal = executor.UpdateCommand(effectiveAt.AddSeconds(5d),
CreateMeasuredState(0d, effectiveAt.AddSeconds(5d), 97L), goalTrajectory, TravelDirection.Forward,
TravelDirection.Forward, false);
AssertCompletedCommand(atGoal, "exact goal terminal command");
AssertCompletedCommand(afterGoal, "after goal terminal command");
}
private static void AssertCompletedCommand(TrajectoryControlCommand command, string name)
{
Verification.NearlyEqual(0d, command.SignedLongitudinalVelocity, name + " signed velocity");
Verification.NearlyEqual(0d, command.YawRate, name + " yaw rate");
Verification.True(command.HoldBrake && command.IsTrajectoryComplete, name + " holds completed trajectory");
}
private static void AssertHeld(GearSwitchStateUpdate update, GearSwitchState expectedState, string name)
{
Verification.Equal(expectedState, update.State, name + " state");