feat: adapt EM trajectories to control commands
This commit is contained in:
梁薄云
@@ -11,6 +11,8 @@ internal static class ExecutorChecks
|
||||
VerifiesGearSwitchSequence(TravelDirection.Forward, TravelDirection.Reverse, "forward-to-reverse");
|
||||
VerifiesGearSwitchSequence(TravelDirection.Reverse, TravelDirection.Forward, "reverse-to-forward");
|
||||
VerifiesExecutorSamplesBoundariesAndCompletesAtSafeTerminals();
|
||||
VerifiesGenericControlCommandMapping();
|
||||
VerifiesVehicleStateProviderRemainsSnapshotOnly();
|
||||
}
|
||||
|
||||
private static void VerifiesGearSwitchSequence(TravelDirection currentDirection, TravelDirection desiredDirection,
|
||||
@@ -98,6 +100,97 @@ internal static class ExecutorChecks
|
||||
}
|
||||
}
|
||||
|
||||
private static void VerifiesGenericControlCommandMapping()
|
||||
{
|
||||
DateTimeOffset effectiveAt = DateTimeOffset.UnixEpoch.AddSeconds(700d);
|
||||
var adapter = new TrajectoryControlAdapter();
|
||||
|
||||
VerifiesFollowingCommand(adapter, effectiveAt, TravelDirection.Forward, 0.12d, 0.35d, 0.042d,
|
||||
"forward command");
|
||||
VerifiesFollowingCommand(adapter, effectiveAt, TravelDirection.Reverse, -0.12d, 0.35d, -0.042d,
|
||||
"reverse command");
|
||||
|
||||
var executor = new TrajectoryExecutor();
|
||||
EmTrajectory gearTrajectory = CreateTrajectory(effectiveAt, TravelDirection.Forward,
|
||||
EmBoundaryType.GearSwitchApproach, EmTerminalType.GearSwitch);
|
||||
executor.Update(effectiveAt, CreateMeasuredState(0.10d, effectiveAt, 80L), gearTrajectory,
|
||||
TravelDirection.Reverse, TravelDirection.Forward, false);
|
||||
TrajectoryExecutionState holding = executor.Update(effectiveAt.AddSeconds(0.30d),
|
||||
CreateMeasuredState(0d, effectiveAt.AddSeconds(0.30d), 81L), gearTrajectory,
|
||||
TravelDirection.Reverse, TravelDirection.Forward, false);
|
||||
TrajectoryControlCommand holdCommand = adapter.CreateCommand(holding.SelectedPoint, holding);
|
||||
Verification.NearlyEqual(0d, holdCommand.SignedLongitudinalVelocity,
|
||||
"holding command overrides signed longitudinal velocity");
|
||||
Verification.NearlyEqual(0d, holdCommand.YawRate, "holding command overrides yaw rate");
|
||||
Verification.True(holdCommand.HoldBrake && !holdCommand.IsTrajectoryComplete,
|
||||
"gear holding command requests brake without completing trajectory");
|
||||
|
||||
TrajectoryExecutionState requesting = executor.Update(effectiveAt.AddSeconds(0.50d),
|
||||
CreateMeasuredState(0d, effectiveAt.AddSeconds(0.50d), 82L), gearTrajectory,
|
||||
TravelDirection.Reverse, TravelDirection.Forward, false);
|
||||
TrajectoryControlCommand requestCommand = adapter.CreateCommand(requesting.SelectedPoint, requesting);
|
||||
Verification.True(requestCommand.RequestDirectionChange && requestCommand.HoldBrake,
|
||||
"direction request remains a generic zero-speed brake command");
|
||||
|
||||
EmTrajectory terminalTrajectory = CreateTrajectory(effectiveAt, TravelDirection.Forward, EmBoundaryType.Goal,
|
||||
EmTerminalType.Goal);
|
||||
var terminalExecutor = new TrajectoryExecutor();
|
||||
TrajectoryExecutionState completed = terminalExecutor.Update(effectiveAt.AddSeconds(0.30d),
|
||||
CreateMeasuredState(0d, effectiveAt.AddSeconds(0.30d), 82L), terminalTrajectory,
|
||||
TravelDirection.Forward, TravelDirection.Forward, false);
|
||||
TrajectoryControlCommand completeCommand = adapter.CreateCommand(completed.SelectedPoint, completed);
|
||||
Verification.NearlyEqual(0d, completeCommand.SignedLongitudinalVelocity,
|
||||
"completed command keeps signed longitudinal velocity at zero");
|
||||
Verification.NearlyEqual(0d, completeCommand.YawRate, "completed command keeps yaw rate at zero");
|
||||
Verification.True(completeCommand.HoldBrake && completeCommand.IsTrajectoryComplete,
|
||||
"completed command holds brake and reports completion");
|
||||
|
||||
Verification.True(typeof(TrajectoryControlCommand).GetProperty("BodyLateralVelocity") == null,
|
||||
"generic command exposes no body lateral velocity");
|
||||
}
|
||||
|
||||
private static void VerifiesFollowingCommand(TrajectoryControlAdapter adapter, DateTimeOffset effectiveAt,
|
||||
TravelDirection direction, double signedVelocity, double curvature, double expectedYawRate, string name)
|
||||
{
|
||||
EmTrajectory trajectory = CreateMotionTrajectory(effectiveAt, direction, signedVelocity, curvature);
|
||||
var executor = new TrajectoryExecutor();
|
||||
VehicleMotionState measured = CreateMeasuredState(signedVelocity, effectiveAt, 83L);
|
||||
TrajectoryExecutionState following = executor.Update(effectiveAt, measured, trajectory, direction, direction, false);
|
||||
TrajectoryControlCommand command = adapter.CreateCommand(following.SelectedPoint, following);
|
||||
TrajectoryControlCommand executorCommand = executor.UpdateCommand(effectiveAt, measured, trajectory, direction,
|
||||
direction, false);
|
||||
|
||||
Verification.NearlyEqual(following.SelectedPoint.SignedLongitudinalVelocity, command.SignedLongitudinalVelocity,
|
||||
name + " copies signed longitudinal velocity exactly");
|
||||
Verification.NearlyEqual(following.SelectedPoint.YawRate, command.YawRate,
|
||||
name + " copies yaw rate exactly");
|
||||
Verification.NearlyEqual(expectedYawRate, command.YawRate, name + " preserves signed yaw rate");
|
||||
Verification.Equal(direction, command.Direction, name + " preserves direction");
|
||||
Verification.True(!command.HoldBrake && !command.RequestDirectionChange && !command.IsTrajectoryComplete,
|
||||
name + " remains a normal following command");
|
||||
Verification.NearlyEqual(command.SignedLongitudinalVelocity, executorCommand.SignedLongitudinalVelocity,
|
||||
name + " executor command preserves signed longitudinal velocity");
|
||||
Verification.NearlyEqual(command.YawRate, executorCommand.YawRate,
|
||||
name + " executor command preserves yaw rate");
|
||||
Verification.True(command.SignedLongitudinalVelocity != 0d || command.YawRate == 0d,
|
||||
name + " never creates in-place rotation");
|
||||
Verification.NearlyEqual(Math.Abs(signedVelocity), following.SelectedPoint.Speed,
|
||||
name + " leaves speed available in execution telemetry");
|
||||
Verification.NearlyEqual(signedVelocity * Math.Cos(following.SelectedPoint.Yaw), following.SelectedPoint.VelocityX,
|
||||
name + " leaves world velocity X in execution telemetry");
|
||||
Verification.NearlyEqual(signedVelocity * Math.Sin(following.SelectedPoint.Yaw), following.SelectedPoint.VelocityY,
|
||||
name + " leaves world velocity Y in execution telemetry");
|
||||
Verification.NearlyEqual(curvature, following.SelectedPoint.VehicleCurvature,
|
||||
name + " leaves curvature available in execution telemetry");
|
||||
}
|
||||
|
||||
private static void VerifiesVehicleStateProviderRemainsSnapshotOnly()
|
||||
{
|
||||
DateTimeOffset capturedAt = DateTimeOffset.UnixEpoch.AddSeconds(710d);
|
||||
IVehicleStateProvider provider = new FixedVehicleStateProvider(CreateMeasuredState(0.02d, capturedAt, 84L));
|
||||
Verification.Equal(84L, provider.Capture().SequenceId, "vehicle state provider returns caller-owned snapshot");
|
||||
}
|
||||
|
||||
private static void AssertHeld(GearSwitchStateUpdate update, GearSwitchState expectedState, string name)
|
||||
{
|
||||
Verification.Equal(expectedState, update.State, name + " state");
|
||||
@@ -128,4 +221,33 @@ internal static class ExecutorChecks
|
||||
terminalBoundary, 0d, 0d),
|
||||
});
|
||||
}
|
||||
|
||||
private static EmTrajectory CreateMotionTrajectory(DateTimeOffset effectiveAt, TravelDirection direction,
|
||||
double signedVelocity, double curvature)
|
||||
{
|
||||
var metadata = new EmTrajectoryMetadata("control-" + direction, effectiveAt, effectiveAt, 85L,
|
||||
"control-reference", 84L, string.Empty, 5, direction, EmTerminalType.Goal);
|
||||
return new EmTrajectory(metadata, new[]
|
||||
{
|
||||
new EmTrajectoryPoint(1d, 2d, Math.PI / 3d, signedVelocity, 0d, curvature, 5, 0d, 0d, direction,
|
||||
EmBoundaryType.None, 0d, 0d),
|
||||
new EmTrajectoryPoint(1d, 2d, Math.PI / 3d, 0d, 0.30d, curvature, 5, 0.04d, 0.04d, direction,
|
||||
EmBoundaryType.Goal, 0d, 0d),
|
||||
});
|
||||
}
|
||||
|
||||
private sealed class FixedVehicleStateProvider : IVehicleStateProvider
|
||||
{
|
||||
private readonly VehicleMotionState state;
|
||||
|
||||
public FixedVehicleStateProvider(VehicleMotionState state)
|
||||
{
|
||||
this.state = state;
|
||||
}
|
||||
|
||||
public VehicleMotionState Capture()
|
||||
{
|
||||
return state;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user