655 lines
22 KiB
C#
655 lines
22 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using MultiWheelC.Control.Abstractions;
|
|
using MultiWheelC.Control.Allocation;
|
|
using MultiWheelC.Fleet;
|
|
using MultiWheelC.Trajectory;
|
|
using MyParking.Shared;
|
|
|
|
namespace MultiWheelC.Tests
|
|
{
|
|
internal static class FleetCoordinatorTests
|
|
{
|
|
private const double Tolerance = 1e-9;
|
|
|
|
public static void Run()
|
|
{
|
|
VerifyCommandCycle();
|
|
VerifySmallLayoutErrorIsCorrected();
|
|
VerifyWarningRangeScalesAllCommands();
|
|
VerifyUnavailableStateStopsAndRecovers();
|
|
VerifyUnsafeLayoutFaultsAndLatches();
|
|
VerifyCompletionStops();
|
|
VerifyTrackingFaultStops();
|
|
VerifyCancelReturnsInactive();
|
|
|
|
Console.WriteLine(
|
|
"FleetCoordinator车队协调测试通过。共8个场景。");
|
|
}
|
|
|
|
private static void VerifyCommandCycle()
|
|
{
|
|
var layout = CreateLayout();
|
|
var coordinator = CreateCoordinator();
|
|
coordinator.Start(layout, CreateTrajectory());
|
|
|
|
var result = coordinator.ExecuteCycle(
|
|
CreateRigidMemberStates(
|
|
layout,
|
|
new Pose2D(0.5, 0.0, 0.0),
|
|
new Twist2D(0.4, 0.0, 0.0),
|
|
1.0),
|
|
targetTimestampSeconds: 1.0,
|
|
deltaTimeSeconds: 0.02,
|
|
out var output);
|
|
|
|
AssertResult(
|
|
result,
|
|
FleetCoordinationCycleResult.CommandGenerated,
|
|
"正常协调周期");
|
|
if (!output.State.HasValue)
|
|
{
|
|
throw new InvalidOperationException(
|
|
"正常协调周期没有返回车队状态。");
|
|
}
|
|
|
|
AssertNear(
|
|
output.State.Value.FleetPoseInWorld.XMeters,
|
|
0.5,
|
|
"正常协调周期中心X");
|
|
AssertNear(
|
|
output.SpeedScale,
|
|
1.0,
|
|
"正常协调周期速度比例");
|
|
AssertTwist(
|
|
output.FleetCommand.TwistAtReferencePoint,
|
|
0.4,
|
|
0.0,
|
|
0.0,
|
|
"正常协调周期车队命令");
|
|
AssertTwist(
|
|
FindCommand(output.MemberCommands, 1)
|
|
.TwistInVehicleBody,
|
|
0.4,
|
|
0.0,
|
|
0.0,
|
|
"正常协调周期车辆1");
|
|
AssertTwist(
|
|
FindCommand(output.MemberCommands, 2)
|
|
.TwistInVehicleBody,
|
|
-0.4,
|
|
0.0,
|
|
0.0,
|
|
"正常协调周期车辆2");
|
|
}
|
|
|
|
private static void VerifySmallLayoutErrorIsCorrected()
|
|
{
|
|
var layout = CreateLayout();
|
|
var coordinator = CreateCoordinator();
|
|
coordinator.Start(layout, CreateTrajectory());
|
|
var result = coordinator.ExecuteCycle(
|
|
new[]
|
|
{
|
|
CreateMemberState(
|
|
1,
|
|
new Pose2D(-0.99, 0.0, 0.0),
|
|
1.0),
|
|
CreateMemberState(
|
|
2,
|
|
new Pose2D(0.99, 0.0, Math.PI),
|
|
1.0)
|
|
},
|
|
targetTimestampSeconds: 1.0,
|
|
deltaTimeSeconds: 0.02,
|
|
out var output);
|
|
|
|
AssertResult(
|
|
result,
|
|
FleetCoordinationCycleResult.CommandGenerated,
|
|
"小范围布局误差纠偏");
|
|
AssertNear(
|
|
output.SpeedScale,
|
|
1.0,
|
|
"小范围布局误差速度比例");
|
|
AssertTwist(
|
|
FindCommand(output.BaseMemberCommands, 1)
|
|
.TwistInVehicleBody,
|
|
0.4,
|
|
0.0,
|
|
0.0,
|
|
"车辆1基础命令");
|
|
AssertTwist(
|
|
FindCommand(output.BaseMemberCommands, 2)
|
|
.TwistInVehicleBody,
|
|
-0.4,
|
|
0.0,
|
|
0.0,
|
|
"车辆2基础命令");
|
|
AssertTwist(
|
|
FindCommand(output.MemberCommands, 1)
|
|
.TwistInVehicleBody,
|
|
0.395,
|
|
0.0,
|
|
0.0,
|
|
"车辆1纠偏后命令");
|
|
AssertTwist(
|
|
FindCommand(output.MemberCommands, 2)
|
|
.TwistInVehicleBody,
|
|
-0.405,
|
|
0.0,
|
|
0.0,
|
|
"车辆2纠偏后命令");
|
|
}
|
|
|
|
private static void VerifyWarningRangeScalesAllCommands()
|
|
{
|
|
var layout = CreateLayout();
|
|
var coordinator = CreateCoordinator();
|
|
coordinator.Start(layout, CreateTrajectory());
|
|
var result = coordinator.ExecuteCycle(
|
|
new[]
|
|
{
|
|
CreateMemberState(
|
|
1,
|
|
new Pose2D(-0.965, 0.0, 0.0),
|
|
1.0),
|
|
CreateMemberState(
|
|
2,
|
|
new Pose2D(0.965, 0.0, Math.PI),
|
|
1.0)
|
|
},
|
|
targetTimestampSeconds: 1.0,
|
|
deltaTimeSeconds: 0.02,
|
|
out var output);
|
|
|
|
AssertResult(
|
|
result,
|
|
FleetCoordinationCycleResult.CommandGenerated,
|
|
"警告区间统一缩放");
|
|
AssertNear(
|
|
output.SpeedScale,
|
|
0.5,
|
|
"警告区间速度比例");
|
|
AssertTwist(
|
|
output.FleetCommand.TwistAtReferencePoint,
|
|
0.2,
|
|
0.0,
|
|
0.0,
|
|
"警告区间车队命令");
|
|
AssertTwist(
|
|
FindCommand(output.MemberCommands, 1)
|
|
.TwistInVehicleBody,
|
|
0.2,
|
|
0.0,
|
|
0.0,
|
|
"警告区间车辆1");
|
|
AssertTwist(
|
|
FindCommand(output.MemberCommands, 2)
|
|
.TwistInVehicleBody,
|
|
-0.2,
|
|
0.0,
|
|
0.0,
|
|
"警告区间车辆2");
|
|
|
|
if (string.IsNullOrWhiteSpace(output.Reason))
|
|
{
|
|
throw new InvalidOperationException(
|
|
"警告区间缩放没有返回限制原因。");
|
|
}
|
|
}
|
|
|
|
private static void VerifyUnavailableStateStopsAndRecovers()
|
|
{
|
|
var layout = CreateLayout();
|
|
var coordinator = CreateCoordinator();
|
|
coordinator.Start(layout, CreateTrajectory());
|
|
var unavailableStates = CreateRigidMemberStates(
|
|
layout,
|
|
new Pose2D(0.5, 0.0, 0.0),
|
|
new Twist2D(0.4, 0.0, 0.0),
|
|
1.0);
|
|
unavailableStates[1] = new FleetMemberStateSample(
|
|
unavailableStates[1].VehicleId,
|
|
unavailableStates[1].SampleTimestampSeconds,
|
|
unavailableStates[1].PoseInWorld,
|
|
unavailableStates[1].TwistAtVehicleOriginInWorld,
|
|
isStateAvailable: false,
|
|
hasValidVelocityEstimate: true);
|
|
|
|
var waitingResult = coordinator.ExecuteCycle(
|
|
unavailableStates,
|
|
targetTimestampSeconds: 1.0,
|
|
deltaTimeSeconds: 0.02,
|
|
out var waitingOutput);
|
|
|
|
AssertResult(
|
|
waitingResult,
|
|
FleetCoordinationCycleResult.WaitingForState,
|
|
"状态暂时不可用");
|
|
AssertStop(waitingOutput, layout.VehicleCount);
|
|
|
|
if (!coordinator.IsActive)
|
|
{
|
|
throw new InvalidOperationException(
|
|
"状态暂时不可用不应取消车队控制器。");
|
|
}
|
|
|
|
var recoveredResult = coordinator.ExecuteCycle(
|
|
CreateRigidMemberStates(
|
|
layout,
|
|
new Pose2D(0.5, 0.0, 0.0),
|
|
new Twist2D(0.4, 0.0, 0.0),
|
|
1.02),
|
|
targetTimestampSeconds: 1.02,
|
|
deltaTimeSeconds: 0.02,
|
|
out _);
|
|
|
|
AssertResult(
|
|
recoveredResult,
|
|
FleetCoordinationCycleResult.CommandGenerated,
|
|
"状态恢复");
|
|
}
|
|
|
|
private static void VerifyUnsafeLayoutFaultsAndLatches()
|
|
{
|
|
var layout = CreateLayout();
|
|
var coordinator = CreateCoordinator();
|
|
coordinator.Start(layout, CreateTrajectory());
|
|
var deformedStates = new[]
|
|
{
|
|
CreateMemberState(
|
|
1,
|
|
new Pose2D(-0.9, 0.0, 0.0),
|
|
1.0),
|
|
CreateMemberState(
|
|
2,
|
|
new Pose2D(0.9, 0.0, Math.PI),
|
|
1.0)
|
|
};
|
|
|
|
var result = coordinator.ExecuteCycle(
|
|
deformedStates,
|
|
targetTimestampSeconds: 1.0,
|
|
deltaTimeSeconds: 0.02,
|
|
out var output);
|
|
|
|
AssertResult(
|
|
result,
|
|
FleetCoordinationCycleResult.Faulted,
|
|
"布局误差超限");
|
|
AssertStop(output, layout.VehicleCount);
|
|
|
|
if (!coordinator.IsFaulted ||
|
|
string.IsNullOrWhiteSpace(
|
|
coordinator.LastFailureReason))
|
|
{
|
|
throw new InvalidOperationException(
|
|
"布局误差故障没有被锁存。");
|
|
}
|
|
|
|
var latchedResult = coordinator.ExecuteCycle(
|
|
CreateRigidMemberStates(
|
|
layout,
|
|
new Pose2D(0.5, 0.0, 0.0),
|
|
new Twist2D(0.4, 0.0, 0.0),
|
|
1.02),
|
|
targetTimestampSeconds: 1.02,
|
|
deltaTimeSeconds: 0.02,
|
|
out var latchedOutput);
|
|
|
|
AssertResult(
|
|
latchedResult,
|
|
FleetCoordinationCycleResult.Faulted,
|
|
"布局误差故障锁存");
|
|
AssertStop(latchedOutput, layout.VehicleCount);
|
|
}
|
|
|
|
private static void VerifyCompletionStops()
|
|
{
|
|
var layout = CreateLayout();
|
|
var coordinator = CreateCoordinator();
|
|
coordinator.Start(layout, CreateTrajectory());
|
|
|
|
var result = coordinator.ExecuteCycle(
|
|
CreateRigidMemberStates(
|
|
layout,
|
|
new Pose2D(1.0, 0.0, 0.0),
|
|
Twist2D.Zero,
|
|
1.0),
|
|
targetTimestampSeconds: 1.0,
|
|
deltaTimeSeconds: 0.02,
|
|
out var output);
|
|
|
|
AssertResult(
|
|
result,
|
|
FleetCoordinationCycleResult.Completed,
|
|
"车队轨迹完成");
|
|
AssertStop(output, layout.VehicleCount);
|
|
|
|
if (!coordinator.IsCompleted)
|
|
{
|
|
throw new InvalidOperationException(
|
|
"车队轨迹完成状态没有被保存。");
|
|
}
|
|
}
|
|
|
|
private static void VerifyTrackingFaultStops()
|
|
{
|
|
var layout = CreateLayout();
|
|
var coordinator = CreateCoordinator();
|
|
coordinator.Start(layout, CreateTrajectory());
|
|
|
|
var result = coordinator.ExecuteCycle(
|
|
CreateRigidMemberStates(
|
|
layout,
|
|
new Pose2D(0.5, 0.5, 0.0),
|
|
Twist2D.Zero,
|
|
1.0),
|
|
targetTimestampSeconds: 1.0,
|
|
deltaTimeSeconds: 0.02,
|
|
out var output);
|
|
|
|
AssertResult(
|
|
result,
|
|
FleetCoordinationCycleResult.Faulted,
|
|
"车队中心跟踪故障");
|
|
AssertStop(output, layout.VehicleCount);
|
|
|
|
if (!coordinator.IsFaulted)
|
|
{
|
|
throw new InvalidOperationException(
|
|
"车队中心跟踪故障没有传递到协调器。");
|
|
}
|
|
}
|
|
|
|
private static void VerifyCancelReturnsInactive()
|
|
{
|
|
var layout = CreateLayout();
|
|
var coordinator = CreateCoordinator();
|
|
coordinator.Start(layout, CreateTrajectory());
|
|
coordinator.Cancel();
|
|
|
|
var result = coordinator.ExecuteCycle(
|
|
CreateRigidMemberStates(
|
|
layout,
|
|
new Pose2D(0.5, 0.0, 0.0),
|
|
Twist2D.Zero,
|
|
1.0),
|
|
targetTimestampSeconds: 1.0,
|
|
deltaTimeSeconds: 0.02,
|
|
out var output);
|
|
|
|
AssertResult(
|
|
result,
|
|
FleetCoordinationCycleResult.Inactive,
|
|
"取消车队协调");
|
|
AssertStop(output, layout.VehicleCount);
|
|
}
|
|
|
|
private static FleetCoordinator CreateCoordinator()
|
|
{
|
|
var estimator = new FleetStateEstimator(
|
|
maximumMemberStateAgeSeconds: 0.25,
|
|
maximumPositionDisagreementMeters: 0.5,
|
|
maximumYawDisagreementRadians:
|
|
AngleMath.DegreesToRadians(10.0));
|
|
var controller = new FleetController(
|
|
new StraightLateralController(),
|
|
new ReferenceLongitudinalController(),
|
|
new GcpCommandAllocator(Math.PI / 4.0),
|
|
virtualControlPointRadiusMeters: 0.5);
|
|
var commandCorrector =
|
|
new FleetMemberCommandCorrector(
|
|
longitudinalPositionGainPerSecond: 1.0,
|
|
lateralPositionGainPerSecond: 1.0,
|
|
yawGainPerSecond: 1.0,
|
|
positionErrorDeadbandMeters: 0.005,
|
|
yawErrorDeadbandRadians:
|
|
AngleMath.DegreesToRadians(0.5),
|
|
maximumLinearCorrectionMetersPerSecond:
|
|
0.03,
|
|
maximumAngularCorrectionRadiansPerSecond:
|
|
AngleMath.DegreesToRadians(2.0));
|
|
|
|
return new FleetCoordinator(
|
|
estimator,
|
|
controller,
|
|
commandCorrector,
|
|
memberPositionErrorWarningMeters: 0.02,
|
|
maximumMemberPositionErrorMeters: 0.05,
|
|
memberYawErrorWarningRadians:
|
|
AngleMath.DegreesToRadians(1.0),
|
|
maximumMemberYawErrorRadians:
|
|
AngleMath.DegreesToRadians(3.0));
|
|
}
|
|
|
|
private static FleetLayout CreateLayout()
|
|
{
|
|
return new FleetLayout(
|
|
new[]
|
|
{
|
|
new VehicleLayout(
|
|
1,
|
|
new Pose2D(-1.0, 0.0, 0.0)),
|
|
new VehicleLayout(
|
|
2,
|
|
new Pose2D(1.0, 0.0, Math.PI))
|
|
});
|
|
}
|
|
|
|
private static Trajectory2D CreateTrajectory()
|
|
{
|
|
return new Trajectory2D(
|
|
new[]
|
|
{
|
|
new TrajectoryPoint(
|
|
0.0,
|
|
Pose2D.Identity,
|
|
0.0,
|
|
0.4),
|
|
new TrajectoryPoint(
|
|
1.0,
|
|
new Pose2D(1.0, 0.0, 0.0),
|
|
0.0,
|
|
0.4)
|
|
});
|
|
}
|
|
|
|
private static FleetMemberStateSample[]
|
|
CreateRigidMemberStates(
|
|
FleetLayout layout,
|
|
Pose2D fleetPoseInWorld,
|
|
Twist2D twistAtFleetOriginInWorld,
|
|
double timestampSeconds)
|
|
{
|
|
var states =
|
|
new FleetMemberStateSample[layout.VehicleCount];
|
|
|
|
for (var index = 0;
|
|
index < layout.Vehicles.Count;
|
|
index++)
|
|
{
|
|
var vehicle = layout.Vehicles[index];
|
|
var poseInWorld = FrameTransform2D.Compose(
|
|
fleetPoseInWorld,
|
|
vehicle.PoseInFleet);
|
|
var offsetX =
|
|
poseInWorld.XMeters -
|
|
fleetPoseInWorld.XMeters;
|
|
var offsetY =
|
|
poseInWorld.YMeters -
|
|
fleetPoseInWorld.YMeters;
|
|
var twistInWorld = new Twist2D(
|
|
twistAtFleetOriginInWorld.VxMetersPerSecond -
|
|
twistAtFleetOriginInWorld.OmegaRadiansPerSecond *
|
|
offsetY,
|
|
twistAtFleetOriginInWorld.VyMetersPerSecond +
|
|
twistAtFleetOriginInWorld.OmegaRadiansPerSecond *
|
|
offsetX,
|
|
twistAtFleetOriginInWorld.OmegaRadiansPerSecond);
|
|
|
|
states[index] = new FleetMemberStateSample(
|
|
vehicle.VehicleId,
|
|
timestampSeconds,
|
|
poseInWorld,
|
|
twistInWorld,
|
|
isStateAvailable: true,
|
|
hasValidVelocityEstimate: true);
|
|
}
|
|
|
|
return states;
|
|
}
|
|
|
|
private static FleetMemberStateSample CreateMemberState(
|
|
int vehicleId,
|
|
Pose2D poseInWorld,
|
|
double timestampSeconds)
|
|
{
|
|
return new FleetMemberStateSample(
|
|
vehicleId,
|
|
timestampSeconds,
|
|
poseInWorld,
|
|
Twist2D.Zero,
|
|
isStateAvailable: true,
|
|
hasValidVelocityEstimate: true);
|
|
}
|
|
|
|
private static FleetMemberCommand FindCommand(
|
|
IReadOnlyList<FleetMemberCommand> commands,
|
|
int vehicleId)
|
|
{
|
|
for (var index = 0;
|
|
index < commands.Count;
|
|
index++)
|
|
{
|
|
if (commands[index].VehicleId == vehicleId)
|
|
{
|
|
return commands[index];
|
|
}
|
|
}
|
|
|
|
throw new InvalidOperationException(
|
|
$"没有找到车辆{vehicleId}的成员命令。");
|
|
}
|
|
|
|
private static void AssertStop(
|
|
FleetCoordinationCycleOutput output,
|
|
int expectedMemberCount)
|
|
{
|
|
AssertTwist(
|
|
output.FleetCommand.TwistAtReferencePoint,
|
|
0.0,
|
|
0.0,
|
|
0.0,
|
|
"车队停止命令");
|
|
|
|
if (output.MemberCommands.Count != expectedMemberCount)
|
|
{
|
|
throw new InvalidOperationException(
|
|
"停止输出的成员命令数量错误。");
|
|
}
|
|
|
|
if (output.BaseMemberCommands.Count != expectedMemberCount)
|
|
{
|
|
throw new InvalidOperationException(
|
|
"停止输出的成员基础命令数量错误。");
|
|
}
|
|
|
|
for (var index = 0;
|
|
index < output.MemberCommands.Count;
|
|
index++)
|
|
{
|
|
AssertTwist(
|
|
output.BaseMemberCommands[index]
|
|
.TwistInVehicleBody,
|
|
0.0,
|
|
0.0,
|
|
0.0,
|
|
"成员基础停止命令");
|
|
AssertTwist(
|
|
output.MemberCommands[index].TwistInVehicleBody,
|
|
0.0,
|
|
0.0,
|
|
0.0,
|
|
"成员停止命令");
|
|
}
|
|
}
|
|
|
|
private static void AssertResult(
|
|
FleetCoordinationCycleResult actual,
|
|
FleetCoordinationCycleResult expected,
|
|
string scenario)
|
|
{
|
|
if (actual != expected)
|
|
{
|
|
throw new InvalidOperationException(
|
|
$"{scenario}结果错误：" +
|
|
$"actual={actual}, expected={expected}。");
|
|
}
|
|
}
|
|
|
|
private static void AssertTwist(
|
|
Twist2D actual,
|
|
double expectedVx,
|
|
double expectedVy,
|
|
double expectedOmega,
|
|
string scenario)
|
|
{
|
|
AssertNear(
|
|
actual.VxMetersPerSecond,
|
|
expectedVx,
|
|
scenario + " Vx");
|
|
AssertNear(
|
|
actual.VyMetersPerSecond,
|
|
expectedVy,
|
|
scenario + " Vy");
|
|
AssertNear(
|
|
actual.OmegaRadiansPerSecond,
|
|
expectedOmega,
|
|
scenario + " Omega");
|
|
}
|
|
|
|
private static void AssertNear(
|
|
double actual,
|
|
double expected,
|
|
string valueName)
|
|
{
|
|
if (Math.Abs(actual - expected) > Tolerance)
|
|
{
|
|
throw new InvalidOperationException(
|
|
$"{valueName}错误：" +
|
|
$"actual={actual:F9}, expected={expected:F9}。");
|
|
}
|
|
}
|
|
|
|
private sealed class StraightLateralController :
|
|
ILateralController
|
|
{
|
|
public LateralControlCommand Compute(
|
|
PathTrackingContext context)
|
|
{
|
|
return LateralControlCommand.Straight;
|
|
}
|
|
|
|
public void Reset()
|
|
{
|
|
}
|
|
}
|
|
|
|
private sealed class ReferenceLongitudinalController :
|
|
ILongitudinalController
|
|
{
|
|
public double ComputeSpeedMetersPerSecond(
|
|
PathTrackingContext context)
|
|
{
|
|
return context.ControlReferenceSpeedMetersPerSecond;
|
|
}
|
|
|
|
public void Reset()
|
|
{
|
|
}
|
|
}
|
|
}
|
|
}
|