test: verify rolling EM execution
This commit is contained in:
@@ -343,3 +343,105 @@ PASS longitudinal-integration
|
|||||||
PASS trajectory
|
PASS trajectory
|
||||||
PASS em-planning-service
|
PASS em-planning-service
|
||||||
```
|
```
|
||||||
|
|
||||||
|
## Rolling execution ownership and deployment
|
||||||
|
|
||||||
|
The first-version boundary has three independently testable layers:
|
||||||
|
|
||||||
|
```text
|
||||||
|
caller snapshots -> EmPlanningCoordinator -> immutable published EmTrajectory
|
||||||
|
-> TrajectoryExecutor -> TrajectoryControlCommand
|
||||||
|
```
|
||||||
|
|
||||||
|
- `EmPlanningService` remains a pure, synchronous, one-shot planner. It consumes only the request snapshot and never
|
||||||
|
reads a clock, current directory, UI, localization, wheel speed, or hardware object.
|
||||||
|
- The caller owns state capture, map/reference-path version selection, the replan clock, and any hardware-specific
|
||||||
|
action after it receives a generic command. `IVehicleStateProvider.Capture()` belongs to this execution boundary and
|
||||||
|
returns a `VehicleMotionState` snapshot; it is not a planner dependency.
|
||||||
|
- `EmPlanningCoordinator` owns latest-wins cycle cancellation and atomic publication. A cycle binds
|
||||||
|
`MapSnapshotId`, `ReferencePathId`, vehicle-state `SequenceId`, `PreviousTrajectoryId`, and `SegmentIndex`; a
|
||||||
|
result publishes only when that complete identity and its version are still current. The default update cadence is
|
||||||
|
`0.20 s` (with the configured `6.0 s` / `5.0 m` planning horizons).
|
||||||
|
- `TrajectoryExecutor` only samples the immutable published trajectory with caller-provided time and measured state.
|
||||||
|
It never extrapolates beyond the final point. A failed replan leaves the last complete published trajectory in
|
||||||
|
service through its exact zero-speed safety tail.
|
||||||
|
|
||||||
|
### Handoff and gear changes
|
||||||
|
|
||||||
|
A normal replan may use a future sample from the prior trajectory only when it is within the configured age and
|
||||||
|
position/yaw/speed tracking tolerances, remains in the same segment and direction, and does not cross a gear boundary.
|
||||||
|
Unsafe tracking, stale data, a terminal boundary, or any segment/direction mismatch causes a deterministic reset to
|
||||||
|
the caller-supplied measured state with no trajectory seed.
|
||||||
|
|
||||||
|
At an exact `GearSwitchApproach` boundary the executor follows this sequence:
|
||||||
|
|
||||||
|
```text
|
||||||
|
Following -> ApproachingGearSwitch -> HoldingZero
|
||||||
|
-> RequestingDirectionChange (one request) -> AwaitingDirectionConfirmation -> Following
|
||||||
|
```
|
||||||
|
|
||||||
|
Measured absolute speed must remain below `0.01 m/s` continuously for `0.20 s` before the single direction-change
|
||||||
|
request. Every holding, direction-confirmation, rolling-stop, and goal-completion command is zero speed and zero yaw
|
||||||
|
rate. `Goal` and `RollingSafetyStop` leave the executor completed while braking is held.
|
||||||
|
|
||||||
|
### Trajectory telemetry and generic command
|
||||||
|
|
||||||
|
Every `EmTrajectoryPoint` retains these fields for execution telemetry and independent validation:
|
||||||
|
|
||||||
|
```text
|
||||||
|
X, Y, Yaw,
|
||||||
|
SignedLongitudinalVelocity, Speed, VelocityX, VelocityY, YawRate, VehicleCurvature,
|
||||||
|
TimeFromStart,
|
||||||
|
SegmentIndex, SegmentLocalS, PathS, Direction, BoundaryType,
|
||||||
|
LongitudinalAcceleration, LongitudinalJerk
|
||||||
|
```
|
||||||
|
|
||||||
|
`SignedLongitudinalVelocity` is authoritative: `Speed = abs(signedV)`, world `VelocityX/Y` are derived from vehicle
|
||||||
|
yaw, and `YawRate = signedV * VehicleCurvature`. Pose, world velocity, speed, and curvature stay available in
|
||||||
|
`TrajectoryExecutionState.SelectedPoint`; they are monitoring telemetry, not controller inputs.
|
||||||
|
|
||||||
|
`TrajectoryControlAdapter` produces only the controller-neutral immutable command below:
|
||||||
|
|
||||||
|
```text
|
||||||
|
SignedLongitudinalVelocity
|
||||||
|
YawRate
|
||||||
|
Direction
|
||||||
|
RequestDirectionChange
|
||||||
|
HoldBrake
|
||||||
|
IsTrajectoryComplete
|
||||||
|
```
|
||||||
|
|
||||||
|
There is intentionally no body-lateral-velocity, crab-motion, in-place-rotation, UI, or hardware field. A later
|
||||||
|
hardware adapter may map this command only after that controller's field semantics are independently confirmed.
|
||||||
|
|
||||||
|
### Windows x64 plugin package
|
||||||
|
|
||||||
|
The build output carries the pinned OSQP runtime and notices. Create the deployable plugin tree from a built managed
|
||||||
|
assembly with an explicit destination that is neither a drive root nor the repository root:
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
& .\ClumsyPilot\scripts\Publish-ClumsyPilotPlugin.ps1 `
|
||||||
|
-ManagedDll .\ClumsyPilot\bin\Debug\netstandard2.0\ClumsyPilot.dll `
|
||||||
|
-OutputDirectory C:\deploy\ParkingRobot
|
||||||
|
```
|
||||||
|
|
||||||
|
The transactional publisher verifies a 64-bit PowerShell host, the OSQP PE machine type, and the pinned
|
||||||
|
`ThirdParty/OSQP/SHA256SUMS` hash before staging and renaming exactly:
|
||||||
|
|
||||||
|
```text
|
||||||
|
plugins/ClumsyPilot.dll
|
||||||
|
plugins/osqp.dll
|
||||||
|
plugins/licenses/OSQP-LICENSE.txt
|
||||||
|
plugins/licenses/OSQP-NOTICE.txt
|
||||||
|
plugins/licenses/OSQP-VERSION.txt
|
||||||
|
```
|
||||||
|
|
||||||
|
Run the complete first-version gate from the repository root:
|
||||||
|
|
||||||
|
```powershell
|
||||||
|
dotnet run --project ClumsyPilot/tests/EMPlannerVerificationHost/EMPlannerVerificationHost.csproj -- em-all
|
||||||
|
```
|
||||||
|
|
||||||
|
Dynamic-obstacle prediction, time-space occupancy, following/yielding/overtaking behavior, dynamic rerouting,
|
||||||
|
body-lateral motion, in-place rotation, UI integration, and hardware integration are explicitly deferred and are not
|
||||||
|
implemented by this first version.
|
||||||
|
|||||||
@@ -19,6 +19,14 @@ internal static class CoordinatorChecks
|
|||||||
VerifiesSafePreviousTrajectoryHandoffs();
|
VerifiesSafePreviousTrajectoryHandoffs();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public static void RunRollingEndToEnd()
|
||||||
|
{
|
||||||
|
VerifiesLatestCycleWinsAndEveryIdentityFieldSuppressesStaleResults();
|
||||||
|
VerifiesSafePreviousTrajectoryHandoffs();
|
||||||
|
VerifiesPublishedForwardAndReverseCommands();
|
||||||
|
VerifiesFailureLeavesPublishedZeroSpeedTailExecutable();
|
||||||
|
}
|
||||||
|
|
||||||
private static void VerifiesCallerSuppliedSchedulingDecision()
|
private static void VerifiesCallerSuppliedSchedulingDecision()
|
||||||
{
|
{
|
||||||
var service = new ControlledPlanningService();
|
var service = new ControlledPlanningService();
|
||||||
@@ -177,6 +185,142 @@ internal static class CoordinatorChecks
|
|||||||
"coordinator consumes only its published immutable trajectory");
|
"coordinator consumes only its published immutable trajectory");
|
||||||
}
|
}
|
||||||
|
|
||||||
|
private static void VerifiesPublishedForwardAndReverseCommands()
|
||||||
|
{
|
||||||
|
DateTimeOffset effectiveAt = DateTimeOffset.UnixEpoch.AddSeconds(800d);
|
||||||
|
var service = new ControlledPlanningService();
|
||||||
|
var coordinator = new EmPlanningCoordinator(service);
|
||||||
|
var executor = new TrajectoryExecutor();
|
||||||
|
|
||||||
|
PlanningCycleInput forwardInput = CreateInput(CreateMap(30), "rolling-reference", 90L, string.Empty, 6,
|
||||||
|
"rolling-forward", effectiveAt, null, CreateMeasuredState(0d, 0d, 0d, 0.10d, effectiveAt, 90L));
|
||||||
|
EmTrajectory forward = CreateExecutionTrajectory("rolling-forward", TravelDirection.Forward, effectiveAt,
|
||||||
|
EmBoundaryType.RollingSafetyStop, EmTerminalType.RollingSafetyStop);
|
||||||
|
PlanningCycleResult forwardResult = Publish(service, coordinator, forwardInput, forward);
|
||||||
|
Verification.True(forwardResult.Published, "first forward rolling cycle publishes");
|
||||||
|
TrajectoryControlCommand forwardCommand = executor.UpdateCommand(effectiveAt,
|
||||||
|
forwardInput.Request.VehicleState, coordinator.PublishedTrajectory, TravelDirection.Forward,
|
||||||
|
TravelDirection.Forward, false);
|
||||||
|
AssertPublishedMotion(forwardCommand, coordinator.PublishedTrajectory.Points[0], "forward rolling command");
|
||||||
|
|
||||||
|
PlanningCycleInput forwardRepeat = CreateInput(CreateMap(31), "rolling-reference", 91L, "rolling-forward", 6,
|
||||||
|
"rolling-forward-repeat", effectiveAt.AddSeconds(0.20d), null,
|
||||||
|
CreateMeasuredState(0.02d, 0d, 0d, 0.10d, effectiveAt.AddSeconds(0.20d), 91L));
|
||||||
|
EmTrajectory repeatedForward = CreateExecutionTrajectory("rolling-forward-repeat", TravelDirection.Forward,
|
||||||
|
effectiveAt.AddSeconds(0.20d), EmBoundaryType.RollingSafetyStop, EmTerminalType.RollingSafetyStop);
|
||||||
|
Publish(service, coordinator, forwardRepeat, repeatedForward);
|
||||||
|
Verification.Equal("rolling-forward-repeat", coordinator.PublishedTrajectory.Metadata.TrajectoryId,
|
||||||
|
"repeated forward replan replaces only the published trajectory");
|
||||||
|
|
||||||
|
PlanningCycleInput reverseInput = CreateInput(CreateMap(32), "rolling-reference", 92L,
|
||||||
|
"rolling-forward-repeat", 7, "rolling-reverse", effectiveAt.AddSeconds(0.40d), null,
|
||||||
|
CreateMeasuredState(0d, 0d, 0d, -0.10d, effectiveAt.AddSeconds(0.40d), 92L));
|
||||||
|
EmTrajectory reverse = CreateExecutionTrajectory("rolling-reverse", TravelDirection.Reverse,
|
||||||
|
effectiveAt.AddSeconds(0.40d), EmBoundaryType.Goal, EmTerminalType.Goal);
|
||||||
|
PlanningCycleResult reverseResult = Publish(service, coordinator, reverseInput, reverse);
|
||||||
|
Verification.True(reverseResult.Published, "reverse rolling cycle publishes");
|
||||||
|
TrajectoryControlCommand reverseCommand = executor.UpdateCommand(effectiveAt.AddSeconds(0.40d),
|
||||||
|
reverseInput.Request.VehicleState, coordinator.PublishedTrajectory, TravelDirection.Reverse,
|
||||||
|
TravelDirection.Reverse, false);
|
||||||
|
AssertPublishedMotion(reverseCommand, coordinator.PublishedTrajectory.Points[0], "reverse rolling command");
|
||||||
|
Verification.True(reverseCommand.SignedLongitudinalVelocity < 0d,
|
||||||
|
"reverse rolling command preserves negative longitudinal velocity");
|
||||||
|
|
||||||
|
PlanningCycleInput unsafeTracking = CreateInput(CreateMap(32), "rolling-reference", 93L, "rolling-reverse", 7,
|
||||||
|
"unsafe-tracking", effectiveAt.AddSeconds(0.41d), CreateHandoffConfiguration(),
|
||||||
|
CreateMeasuredState(5d, 0d, 0d, -0.10d, effectiveAt.AddSeconds(0.41d), 93L));
|
||||||
|
TrajectoryHandoffSelection reset = coordinator.SelectHandoff(unsafeTracking, TravelDirection.Reverse);
|
||||||
|
Verification.Equal(TrajectoryHandoffSource.MeasuredState, reset.Source,
|
||||||
|
"unsafe tracking resets rolling handoff to caller measurement");
|
||||||
|
Verification.True(object.ReferenceEquals(unsafeTracking.Request.VehicleState, reset.StartState),
|
||||||
|
"unsafe tracking preserves the supplied measurement snapshot");
|
||||||
|
}
|
||||||
|
|
||||||
|
private static void VerifiesFailureLeavesPublishedZeroSpeedTailExecutable()
|
||||||
|
{
|
||||||
|
DateTimeOffset effectiveAt = DateTimeOffset.UnixEpoch.AddSeconds(900d);
|
||||||
|
var service = new ControlledPlanningService();
|
||||||
|
var coordinator = new EmPlanningCoordinator(service);
|
||||||
|
var executor = new TrajectoryExecutor();
|
||||||
|
PlanningCycleInput successfulInput = CreateInput(CreateMap(40), "tail-reference", 100L, string.Empty, 8,
|
||||||
|
"tail-published", effectiveAt, null, CreateMeasuredState(0d, 0d, 0d, 0.10d, effectiveAt, 100L));
|
||||||
|
EmTrajectory published = CreateExecutionTrajectory("tail-published", TravelDirection.Forward, effectiveAt,
|
||||||
|
EmBoundaryType.RollingSafetyStop, EmTerminalType.RollingSafetyStop);
|
||||||
|
Publish(service, coordinator, successfulInput, published);
|
||||||
|
|
||||||
|
for (int attempt = 1; attempt <= 2; attempt++)
|
||||||
|
{
|
||||||
|
DateTimeOffset now = effectiveAt.AddSeconds(0.20d * attempt);
|
||||||
|
PlanningCycleInput failedInput = CreateInput(CreateMap(40 + attempt), "tail-reference", 100L + attempt,
|
||||||
|
"tail-published", 8, "tail-failed-" + attempt, now, null,
|
||||||
|
CreateMeasuredState(0.02d, 0d, 0d, 0.10d, now, 100L + attempt));
|
||||||
|
Task<PlanningCycleResult> failedCycle = coordinator.PlanLatestAsync(failedInput, CancellationToken.None);
|
||||||
|
service.WaitUntilStarted(failedInput.Request.OutputTrajectoryId);
|
||||||
|
service.Fail(failedInput.Request.OutputTrajectoryId);
|
||||||
|
PlanningCycleResult failure = failedCycle.GetAwaiter().GetResult();
|
||||||
|
Verification.Equal(EmPlanningStatus.Failed, failure.Result.Status, "failed replan status " + attempt);
|
||||||
|
Verification.True(!failure.Published, "failed replan cannot publish " + attempt);
|
||||||
|
Verification.True(object.ReferenceEquals(published, coordinator.PublishedTrajectory),
|
||||||
|
"failed replan retains the complete prior trajectory " + attempt);
|
||||||
|
}
|
||||||
|
|
||||||
|
TrajectoryControlCommand beforeTail = executor.UpdateCommand(effectiveAt.AddSeconds(0.10d),
|
||||||
|
CreateMeasuredState(0.10d, 0d, 0d, 0.10d, effectiveAt.AddSeconds(0.10d), 103L),
|
||||||
|
coordinator.PublishedTrajectory, TravelDirection.Forward, TravelDirection.Forward, false);
|
||||||
|
Verification.True(beforeTail.SignedLongitudinalVelocity > 0d,
|
||||||
|
"old trajectory remains executable before its safety tail");
|
||||||
|
|
||||||
|
TrajectoryControlCommand atTail = executor.UpdateCommand(effectiveAt.AddSeconds(0.30d),
|
||||||
|
CreateMeasuredState(0d, 0d, 0d, 0d, effectiveAt.AddSeconds(0.30d), 104L), coordinator.PublishedTrajectory,
|
||||||
|
TravelDirection.Forward, TravelDirection.Forward, false);
|
||||||
|
TrajectoryControlCommand afterTail = executor.UpdateCommand(effectiveAt.AddSeconds(10d),
|
||||||
|
CreateMeasuredState(0d, 0d, 0d, 0d, effectiveAt.AddSeconds(10d), 105L), coordinator.PublishedTrajectory,
|
||||||
|
TravelDirection.Forward, TravelDirection.Forward, false);
|
||||||
|
AssertZeroTail(atTail, "exact rolling safety-stop terminal");
|
||||||
|
AssertZeroTail(afterTail, "after rolling safety-stop terminal");
|
||||||
|
}
|
||||||
|
|
||||||
|
private static PlanningCycleResult Publish(ControlledPlanningService service, EmPlanningCoordinator coordinator,
|
||||||
|
PlanningCycleInput input, EmTrajectory trajectory)
|
||||||
|
{
|
||||||
|
Task<PlanningCycleResult> cycle = coordinator.PlanLatestAsync(input, CancellationToken.None);
|
||||||
|
service.WaitUntilStarted(input.Request.OutputTrajectoryId);
|
||||||
|
service.Complete(input.Request.OutputTrajectoryId, trajectory);
|
||||||
|
return cycle.GetAwaiter().GetResult();
|
||||||
|
}
|
||||||
|
|
||||||
|
private static EmTrajectory CreateExecutionTrajectory(string trajectoryId, TravelDirection direction,
|
||||||
|
DateTimeOffset effectiveAt, EmBoundaryType terminalBoundary, EmTerminalType terminalType)
|
||||||
|
{
|
||||||
|
double signedVelocity = direction == TravelDirection.Forward ? 0.10d : -0.10d;
|
||||||
|
var metadata = new EmTrajectoryMetadata(trajectoryId, effectiveAt, effectiveAt, 110L, "rolling-reference", 100L,
|
||||||
|
string.Empty, 8, direction, terminalType);
|
||||||
|
return new EmTrajectory(metadata, new[]
|
||||||
|
{
|
||||||
|
new EmTrajectoryPoint(0d, 0d, 0d, signedVelocity, 0d, 0.20d, 8, 0d, 0d, direction,
|
||||||
|
EmBoundaryType.None, 0d, 0d),
|
||||||
|
new EmTrajectoryPoint(signedVelocity * 3d, 0d, 0d, 0d, 0.30d, 0.20d, 8, 0.03d, 0.03d, direction,
|
||||||
|
terminalBoundary, 0d, 0d),
|
||||||
|
});
|
||||||
|
}
|
||||||
|
|
||||||
|
private static void AssertPublishedMotion(TrajectoryControlCommand command, EmTrajectoryPoint point, string name)
|
||||||
|
{
|
||||||
|
Verification.NearlyEqual(point.SignedLongitudinalVelocity, command.SignedLongitudinalVelocity,
|
||||||
|
name + " originates from the currently published trajectory");
|
||||||
|
Verification.NearlyEqual(point.YawRate, command.YawRate,
|
||||||
|
name + " preserves the currently published trajectory yaw rate");
|
||||||
|
Verification.True(!command.HoldBrake && !command.IsTrajectoryComplete,
|
||||||
|
name + " is neither a brake hold nor an extrapolated completion command");
|
||||||
|
}
|
||||||
|
|
||||||
|
private static void AssertZeroTail(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 a completed trajectory");
|
||||||
|
}
|
||||||
|
|
||||||
private static void AssertHandoffRejected(TrajectoryHandoffSelector selector, EmTrajectory trajectory,
|
private static void AssertHandoffRejected(TrajectoryHandoffSelector selector, EmTrajectory trajectory,
|
||||||
VehicleMotionState measuredState, int segmentIndex, TravelDirection direction, DateTimeOffset now,
|
VehicleMotionState measuredState, int segmentIndex, TravelDirection direction, DateTimeOffset now,
|
||||||
EmPlannerConfiguration configuration, TrajectoryHandoffRejectionReason reason, string name)
|
EmPlannerConfiguration configuration, TrajectoryHandoffRejectionReason reason, string name)
|
||||||
@@ -320,6 +464,14 @@ internal static class CoordinatorChecks
|
|||||||
cycle.Completion.TrySetResult(result);
|
cycle.Completion.TrySetResult(result);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
public void Fail(string outputTrajectoryId)
|
||||||
|
{
|
||||||
|
PendingCycle cycle;
|
||||||
|
lock (gate)
|
||||||
|
cycle = pending[outputTrajectoryId];
|
||||||
|
cycle.Completion.TrySetResult(new EmPlanningResult(EmPlanningStatus.Failed, null, "scripted rolling failure"));
|
||||||
|
}
|
||||||
|
|
||||||
private static EmPlanningResult CreateSuccess(EmPlanningRequest request)
|
private static EmPlanningResult CreateSuccess(EmPlanningRequest request)
|
||||||
{
|
{
|
||||||
var metadata = new EmTrajectoryMetadata(request.OutputTrajectoryId, request.RequestedAtUtc, request.EffectiveAtUtc,
|
var metadata = new EmTrajectoryMetadata(request.OutputTrajectoryId, request.RequestedAtUtc, request.EffectiveAtUtc,
|
||||||
|
|||||||
@@ -15,6 +15,15 @@ internal static class ExecutorChecks
|
|||||||
VerifiesVehicleStateProviderRemainsSnapshotOnly();
|
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,
|
private static void VerifiesGearSwitchSequence(TravelDirection currentDirection, TravelDirection desiredDirection,
|
||||||
string name)
|
string name)
|
||||||
{
|
{
|
||||||
@@ -191,6 +200,36 @@ internal static class ExecutorChecks
|
|||||||
Verification.Equal(84L, provider.Capture().SequenceId, "vehicle state provider returns caller-owned snapshot");
|
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)
|
private static void AssertHeld(GearSwitchStateUpdate update, GearSwitchState expectedState, string name)
|
||||||
{
|
{
|
||||||
Verification.Equal(expectedState, update.State, name + " state");
|
Verification.Equal(expectedState, update.State, name + " state");
|
||||||
|
|||||||
@@ -48,7 +48,11 @@ internal static class LateralIntegrationChecks
|
|||||||
{
|
{
|
||||||
Directory.CreateDirectory(pluginDirectory);
|
Directory.CreateDirectory(pluginDirectory);
|
||||||
foreach (string sourcePath in Directory.GetFiles(AppContext.BaseDirectory))
|
foreach (string sourcePath in Directory.GetFiles(AppContext.BaseDirectory))
|
||||||
|
{
|
||||||
|
if (string.Equals(Path.GetFileName(sourcePath), "osqp.dll", StringComparison.OrdinalIgnoreCase))
|
||||||
|
continue;
|
||||||
File.Copy(sourcePath, Path.Combine(pluginDirectory, Path.GetFileName(sourcePath)), false);
|
File.Copy(sourcePath, Path.Combine(pluginDirectory, Path.GetFileName(sourcePath)), false);
|
||||||
|
}
|
||||||
string nativeSource = Path.GetFullPath(Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot", "ThirdParty",
|
string nativeSource = Path.GetFullPath(Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot", "ThirdParty",
|
||||||
"OSQP", "win-x64", "osqp.dll"));
|
"OSQP", "win-x64", "osqp.dll"));
|
||||||
Verification.True(File.Exists(nativeSource), "pinned OSQP DLL is available for the real lateral bundle");
|
Verification.True(File.Exists(nativeSource), "pinned OSQP DLL is available for the real lateral bundle");
|
||||||
|
|||||||
@@ -86,6 +86,8 @@ internal static class OsqpChecks
|
|||||||
string[] hostFiles = Directory.GetFiles(sourceDirectory);
|
string[] hostFiles = Directory.GetFiles(sourceDirectory);
|
||||||
for (int index = 0; index < hostFiles.Length; index++)
|
for (int index = 0; index < hostFiles.Length; index++)
|
||||||
{
|
{
|
||||||
|
if (string.Equals(Path.GetFileName(hostFiles[index]), "osqp.dll", StringComparison.OrdinalIgnoreCase))
|
||||||
|
continue;
|
||||||
string destinationPath = Path.Combine(destinationDirectory, Path.GetFileName(hostFiles[index]));
|
string destinationPath = Path.Combine(destinationDirectory, Path.GetFileName(hostFiles[index]));
|
||||||
File.Copy(hostFiles[index], destinationPath, false);
|
File.Copy(hostFiles[index], destinationPath, false);
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -13,40 +13,40 @@ internal static class Program
|
|||||||
args[0] != "longitudinal-model" && args[0] != "longitudinal-integration" &&
|
args[0] != "longitudinal-model" && args[0] != "longitudinal-integration" &&
|
||||||
args[0] != "longitudinal-real-osqp-probe" && args[0] != "trajectory" &&
|
args[0] != "longitudinal-real-osqp-probe" && args[0] != "trajectory" &&
|
||||||
args[0] != "em-planning-service" && args[0] != "em-core-all" && args[0] != "coordinator" &&
|
args[0] != "em-planning-service" && args[0] != "em-core-all" && args[0] != "coordinator" &&
|
||||||
args[0] != "executor" && args[0] != "plugin-package"))
|
args[0] != "executor" && args[0] != "plugin-package" && args[0] != "em-all"))
|
||||||
{
|
{
|
||||||
Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|osqp|osqp-loader|all-foundation|lateral-model|lateral-integration|lateral-real-osqp|lateral-all|longitudinal-model|longitudinal-integration");
|
Console.Error.WriteLine("Usage: EMPlannerVerificationHost foundation|segmentation|frenet|corridor|optimization|osqp|osqp-loader|all-foundation|lateral-model|lateral-integration|lateral-real-osqp|lateral-all|longitudinal-model|longitudinal-integration|em-core-all|coordinator|executor|plugin-package|em-all");
|
||||||
return 2;
|
return 2;
|
||||||
}
|
}
|
||||||
|
|
||||||
try
|
try
|
||||||
{
|
{
|
||||||
if (args[0] == "foundation" || args[0] == "all-foundation")
|
if (args[0] == "foundation" || args[0] == "all-foundation" || args[0] == "em-all")
|
||||||
{
|
{
|
||||||
MultiWheelC.TrajectoryPlanning.EMPlanner.FoundationChecks.Run();
|
MultiWheelC.TrajectoryPlanning.EMPlanner.FoundationChecks.Run();
|
||||||
Console.WriteLine("PASS foundation");
|
Console.WriteLine("PASS foundation");
|
||||||
}
|
}
|
||||||
if (args[0] == "segmentation" || args[0] == "all-foundation")
|
if (args[0] == "segmentation" || args[0] == "all-foundation" || args[0] == "em-all")
|
||||||
{
|
{
|
||||||
MultiWheelC.TrajectoryPlanning.EMPlanner.SegmentationChecks.Run();
|
MultiWheelC.TrajectoryPlanning.EMPlanner.SegmentationChecks.Run();
|
||||||
Console.WriteLine("PASS segmentation");
|
Console.WriteLine("PASS segmentation");
|
||||||
}
|
}
|
||||||
if (args[0] == "frenet" || args[0] == "all-foundation")
|
if (args[0] == "frenet" || args[0] == "all-foundation" || args[0] == "em-all")
|
||||||
{
|
{
|
||||||
MultiWheelC.TrajectoryPlanning.EMPlanner.FrenetChecks.Run();
|
MultiWheelC.TrajectoryPlanning.EMPlanner.FrenetChecks.Run();
|
||||||
Console.WriteLine("PASS frenet");
|
Console.WriteLine("PASS frenet");
|
||||||
}
|
}
|
||||||
if (args[0] == "corridor" || args[0] == "all-foundation")
|
if (args[0] == "corridor" || args[0] == "all-foundation" || args[0] == "em-all")
|
||||||
{
|
{
|
||||||
MultiWheelC.TrajectoryPlanning.EMPlanner.CorridorChecks.Run();
|
MultiWheelC.TrajectoryPlanning.EMPlanner.CorridorChecks.Run();
|
||||||
Console.WriteLine("PASS corridor");
|
Console.WriteLine("PASS corridor");
|
||||||
}
|
}
|
||||||
if (args[0] == "optimization")
|
if (args[0] == "optimization" || args[0] == "em-all")
|
||||||
{
|
{
|
||||||
MultiWheelC.TrajectoryPlanning.EMPlanner.OptimizationChecks.Run();
|
MultiWheelC.TrajectoryPlanning.EMPlanner.OptimizationChecks.Run();
|
||||||
Console.WriteLine("PASS optimization");
|
Console.WriteLine("PASS optimization");
|
||||||
}
|
}
|
||||||
if (args[0] == "osqp" || args[0] == "osqp-loader")
|
if (args[0] == "osqp" || args[0] == "osqp-loader" || args[0] == "em-all")
|
||||||
{
|
{
|
||||||
MultiWheelC.TrajectoryPlanning.EMPlanner.OsqpChecks.Run();
|
MultiWheelC.TrajectoryPlanning.EMPlanner.OsqpChecks.Run();
|
||||||
Console.WriteLine("PASS osqp-loader");
|
Console.WriteLine("PASS osqp-loader");
|
||||||
@@ -55,12 +55,12 @@ internal static class Program
|
|||||||
{
|
{
|
||||||
MultiWheelC.TrajectoryPlanning.EMPlanner.OsqpChecks.RunProbe();
|
MultiWheelC.TrajectoryPlanning.EMPlanner.OsqpChecks.RunProbe();
|
||||||
}
|
}
|
||||||
if (args[0] == "lateral-model" || args[0] == "lateral-all")
|
if (args[0] == "lateral-model" || args[0] == "lateral-all" || args[0] == "em-all")
|
||||||
{
|
{
|
||||||
MultiWheelC.TrajectoryPlanning.EMPlanner.LateralModelChecks.Run();
|
MultiWheelC.TrajectoryPlanning.EMPlanner.LateralModelChecks.Run();
|
||||||
Console.WriteLine("PASS lateral-model");
|
Console.WriteLine("PASS lateral-model");
|
||||||
}
|
}
|
||||||
if (args[0] == "lateral-integration" || args[0] == "lateral-all")
|
if (args[0] == "lateral-integration" || args[0] == "lateral-all" || args[0] == "em-all")
|
||||||
{
|
{
|
||||||
MultiWheelC.TrajectoryPlanning.EMPlanner.LateralIntegrationChecks.Run();
|
MultiWheelC.TrajectoryPlanning.EMPlanner.LateralIntegrationChecks.Run();
|
||||||
Console.WriteLine("PASS lateral-integration");
|
Console.WriteLine("PASS lateral-integration");
|
||||||
@@ -70,7 +70,7 @@ internal static class Program
|
|||||||
MultiWheelC.TrajectoryPlanning.EMPlanner.LateralIntegrationChecks.RunRealOsqp();
|
MultiWheelC.TrajectoryPlanning.EMPlanner.LateralIntegrationChecks.RunRealOsqp();
|
||||||
Console.WriteLine("PASS lateral-real-osqp");
|
Console.WriteLine("PASS lateral-real-osqp");
|
||||||
}
|
}
|
||||||
if (args[0] == "lateral-real-osqp" || args[0] == "lateral-all")
|
if (args[0] == "lateral-real-osqp" || args[0] == "lateral-all" || args[0] == "em-all")
|
||||||
{
|
{
|
||||||
MultiWheelC.TrajectoryPlanning.EMPlanner.LateralIntegrationChecks.RunRealOsqpInCleanPluginBundle();
|
MultiWheelC.TrajectoryPlanning.EMPlanner.LateralIntegrationChecks.RunRealOsqpInCleanPluginBundle();
|
||||||
Console.WriteLine("PASS lateral-real-osqp");
|
Console.WriteLine("PASS lateral-real-osqp");
|
||||||
@@ -95,9 +95,9 @@ internal static class Program
|
|||||||
MultiWheelC.TrajectoryPlanning.EMPlanner.TrajectoryChecks.Run();
|
MultiWheelC.TrajectoryPlanning.EMPlanner.TrajectoryChecks.Run();
|
||||||
Console.WriteLine("PASS trajectory");
|
Console.WriteLine("PASS trajectory");
|
||||||
}
|
}
|
||||||
if (args[0] == "em-planning-service" || args[0] == "em-core-all")
|
if (args[0] == "em-planning-service" || args[0] == "em-core-all" || args[0] == "em-all")
|
||||||
{
|
{
|
||||||
if (args[0] == "em-core-all")
|
if (args[0] == "em-core-all" || args[0] == "em-all")
|
||||||
{
|
{
|
||||||
MultiWheelC.TrajectoryPlanning.EMPlanner.LongitudinalModelChecks.Run();
|
MultiWheelC.TrajectoryPlanning.EMPlanner.LongitudinalModelChecks.Run();
|
||||||
Console.WriteLine("PASS longitudinal-model");
|
Console.WriteLine("PASS longitudinal-model");
|
||||||
@@ -109,21 +109,28 @@ internal static class Program
|
|||||||
MultiWheelC.TrajectoryPlanning.EMPlanner.EmPlanningServiceChecks.Run();
|
MultiWheelC.TrajectoryPlanning.EMPlanner.EmPlanningServiceChecks.Run();
|
||||||
Console.WriteLine("PASS em-planning-service");
|
Console.WriteLine("PASS em-planning-service");
|
||||||
}
|
}
|
||||||
if (args[0] == "coordinator")
|
if (args[0] == "coordinator" || args[0] == "em-all")
|
||||||
{
|
{
|
||||||
CoordinatorChecks.Run();
|
CoordinatorChecks.Run();
|
||||||
Console.WriteLine("PASS coordinator");
|
Console.WriteLine("PASS coordinator");
|
||||||
}
|
}
|
||||||
if (args[0] == "executor")
|
if (args[0] == "executor" || args[0] == "em-all")
|
||||||
{
|
{
|
||||||
ExecutorChecks.Run();
|
ExecutorChecks.Run();
|
||||||
Console.WriteLine("PASS executor");
|
Console.WriteLine("PASS executor");
|
||||||
}
|
}
|
||||||
if (args[0] == "plugin-package")
|
if (args[0] == "plugin-package" || args[0] == "em-all")
|
||||||
{
|
{
|
||||||
PluginPackagingChecks.Run();
|
PluginPackagingChecks.Run();
|
||||||
Console.WriteLine("PASS plugin-package");
|
Console.WriteLine("PASS plugin-package");
|
||||||
}
|
}
|
||||||
|
if (args[0] == "em-all")
|
||||||
|
{
|
||||||
|
CoordinatorChecks.RunRollingEndToEnd();
|
||||||
|
Console.WriteLine("PASS rolling-end-to-end");
|
||||||
|
ExecutorChecks.RunRollingEndToEnd();
|
||||||
|
Console.WriteLine("PASS rolling-execution-tail");
|
||||||
|
}
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
catch (Exception exception)
|
catch (Exception exception)
|
||||||
|
|||||||
Reference in New Issue
Block a user