完善状态估计、车队协调与舵轮辨识日志
This commit is contained in:
@@ -15,13 +15,15 @@ namespace MultiWheelC.Tests
|
||||
{
|
||||
VerifyInactiveControllerStops();
|
||||
VerifyStraightCommand();
|
||||
VerifyFortyFiveDegreeMotionDirection();
|
||||
VerifyNinetyDegreeMotionDirection();
|
||||
VerifyInvalidVelocityUsesReferenceSpeed();
|
||||
VerifyCompletionStops();
|
||||
VerifyExcessiveTrackingErrorFaults();
|
||||
VerifyCancelStops();
|
||||
|
||||
Console.WriteLine(
|
||||
"FleetController车队中心控制测试通过。共6个场景。");
|
||||
"FleetController车队中心控制测试通过。共8个场景。");
|
||||
}
|
||||
|
||||
private static void VerifyInactiveControllerStops()
|
||||
@@ -91,6 +93,56 @@ namespace MultiWheelC.Tests
|
||||
"速度尚未初始化");
|
||||
}
|
||||
|
||||
private static void VerifyFortyFiveDegreeMotionDirection()
|
||||
{
|
||||
VerifyMotionDirection(
|
||||
Math.PI / 4.0,
|
||||
"45度运动方向");
|
||||
}
|
||||
|
||||
private static void VerifyNinetyDegreeMotionDirection()
|
||||
{
|
||||
VerifyMotionDirection(
|
||||
Math.PI / 2.0,
|
||||
"90度运动方向");
|
||||
}
|
||||
|
||||
private static void VerifyMotionDirection(
|
||||
double motionDirectionInFleetRadians,
|
||||
string scenario)
|
||||
{
|
||||
var controller = CreateController(
|
||||
motionDirectionInFleetRadians);
|
||||
controller.Start(
|
||||
CreateStraightTrajectory(
|
||||
motionDirectionInFleetRadians));
|
||||
|
||||
var directionX =
|
||||
Math.Cos(motionDirectionInFleetRadians);
|
||||
var directionY =
|
||||
Math.Sin(motionDirectionInFleetRadians);
|
||||
var result = controller.ComputeCommand(
|
||||
CreateState(
|
||||
0.5 * directionX,
|
||||
0.5 * directionY,
|
||||
0.4 * directionX,
|
||||
0.4 * directionY,
|
||||
true),
|
||||
0.02,
|
||||
out var command);
|
||||
|
||||
AssertResult(
|
||||
result,
|
||||
FleetControlCycleResult.CommandGenerated,
|
||||
scenario);
|
||||
AssertTwist(
|
||||
command.TwistAtReferencePoint,
|
||||
0.4 * directionX,
|
||||
0.4 * directionY,
|
||||
0.0,
|
||||
scenario);
|
||||
}
|
||||
|
||||
private static void VerifyCompletionStops()
|
||||
{
|
||||
var controller = CreateController();
|
||||
@@ -156,18 +208,27 @@ namespace MultiWheelC.Tests
|
||||
AssertStop(command, "取消控制");
|
||||
}
|
||||
|
||||
private static FleetController CreateController()
|
||||
private static FleetController CreateController(
|
||||
double motionDirectionInFleetRadians = 0.0)
|
||||
{
|
||||
return new FleetController(
|
||||
new StraightLateralController(),
|
||||
new ReferenceLongitudinalController(),
|
||||
new GcpCommandAllocator(
|
||||
Math.PI / 4.0),
|
||||
virtualControlPointRadiusMeters: 0.5);
|
||||
virtualControlPointRadiusMeters: 0.5,
|
||||
motionDirectionInFleetRadians:
|
||||
motionDirectionInFleetRadians);
|
||||
}
|
||||
|
||||
private static Trajectory2D CreateStraightTrajectory()
|
||||
private static Trajectory2D CreateStraightTrajectory(
|
||||
double motionDirectionInFleetRadians = 0.0)
|
||||
{
|
||||
var directionX =
|
||||
Math.Cos(motionDirectionInFleetRadians);
|
||||
var directionY =
|
||||
Math.Sin(motionDirectionInFleetRadians);
|
||||
|
||||
return new Trajectory2D(
|
||||
new[]
|
||||
{
|
||||
@@ -178,7 +239,10 @@ namespace MultiWheelC.Tests
|
||||
0.4),
|
||||
new TrajectoryPoint(
|
||||
1.0,
|
||||
new Pose2D(1.0, 0.0, 0.0),
|
||||
new Pose2D(
|
||||
directionX,
|
||||
directionY,
|
||||
0.0),
|
||||
0.0,
|
||||
0.4)
|
||||
});
|
||||
@@ -189,6 +253,21 @@ namespace MultiWheelC.Tests
|
||||
double yMeters,
|
||||
double vxMetersPerSecond,
|
||||
bool hasValidVelocityEstimate)
|
||||
{
|
||||
return CreateState(
|
||||
xMeters,
|
||||
yMeters,
|
||||
vxMetersPerSecond,
|
||||
0.0,
|
||||
hasValidVelocityEstimate);
|
||||
}
|
||||
|
||||
private static FleetState CreateState(
|
||||
double xMeters,
|
||||
double yMeters,
|
||||
double vxMetersPerSecond,
|
||||
double vyMetersPerSecond,
|
||||
bool hasValidVelocityEstimate)
|
||||
{
|
||||
return new FleetState(
|
||||
sampleTimestampSeconds: 1.0,
|
||||
@@ -198,7 +277,7 @@ namespace MultiWheelC.Tests
|
||||
0.0),
|
||||
twistAtFleetOriginInWorld: new Twist2D(
|
||||
vxMetersPerSecond,
|
||||
0.0,
|
||||
vyMetersPerSecond,
|
||||
0.0),
|
||||
hasValidVelocityEstimate:
|
||||
hasValidVelocityEstimate);
|
||||
|
||||
@@ -0,0 +1,654 @@
|
||||
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()
|
||||
{
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,268 @@
|
||||
using System;
|
||||
using MultiWheelC.Fleet;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Tests
|
||||
{
|
||||
internal static class FleetLayoutCaptureTests
|
||||
{
|
||||
private const double Tolerance = 1e-9;
|
||||
|
||||
public static void Run()
|
||||
{
|
||||
VerifySymmetricTailToTailLayout();
|
||||
VerifyAsymmetricLayoutAndPoseReconstruction();
|
||||
VerifyInputOrderDoesNotChangeResult();
|
||||
VerifyEmptyInputIsRejected();
|
||||
VerifyDuplicateVehicleIdIsRejected();
|
||||
VerifyMissingLeaderIsRejected();
|
||||
|
||||
Console.WriteLine(
|
||||
"FleetLayoutCapture布局建立测试通过。共6个场景。");
|
||||
}
|
||||
|
||||
private static void VerifySymmetricTailToTailLayout()
|
||||
{
|
||||
var result = FleetLayoutCapture.Capture(
|
||||
new[]
|
||||
{
|
||||
new FleetMemberPose(
|
||||
1,
|
||||
new Pose2D(-1.2, 0.0, 0.0)),
|
||||
new FleetMemberPose(
|
||||
2,
|
||||
new Pose2D(1.2, 0.0, Math.PI))
|
||||
},
|
||||
leaderVehicleId: 1);
|
||||
|
||||
AssertPose(
|
||||
result.FleetPoseInWorld,
|
||||
Pose2D.Identity,
|
||||
"对称双车中心");
|
||||
AssertVehicleLayout(
|
||||
result.Layout,
|
||||
1,
|
||||
new Pose2D(-1.2, 0.0, 0.0),
|
||||
"对称双车主车布局");
|
||||
AssertVehicleLayout(
|
||||
result.Layout,
|
||||
2,
|
||||
new Pose2D(1.2, 0.0, Math.PI),
|
||||
"对称双车从车布局");
|
||||
}
|
||||
|
||||
private static void VerifyAsymmetricLayoutAndPoseReconstruction()
|
||||
{
|
||||
var members = new[]
|
||||
{
|
||||
new FleetMemberPose(
|
||||
3,
|
||||
new Pose2D(1.0, 1.0, 0.4)),
|
||||
new FleetMemberPose(
|
||||
1,
|
||||
new Pose2D(4.0, 1.0, 0.4)),
|
||||
new FleetMemberPose(
|
||||
2,
|
||||
new Pose2D(1.0, 4.0, -0.8))
|
||||
};
|
||||
var result = FleetLayoutCapture.Capture(
|
||||
members,
|
||||
leaderVehicleId: 1);
|
||||
|
||||
AssertPose(
|
||||
result.FleetPoseInWorld,
|
||||
new Pose2D(2.0, 2.0, 0.4),
|
||||
"非对称三车中心");
|
||||
|
||||
for (var index = 0;
|
||||
index < members.Length;
|
||||
index++)
|
||||
{
|
||||
if (!result.Layout.TryGetVehicle(
|
||||
members[index].VehicleId,
|
||||
out var vehicleLayout))
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"非对称布局缺少成员车。" +
|
||||
members[index].VehicleId);
|
||||
}
|
||||
|
||||
var reconstructedPoseInWorld =
|
||||
FrameTransform2D.Compose(
|
||||
result.FleetPoseInWorld,
|
||||
vehicleLayout.PoseInFleet);
|
||||
AssertPose(
|
||||
reconstructedPoseInWorld,
|
||||
members[index].PoseInWorld,
|
||||
"非对称布局世界位姿还原");
|
||||
}
|
||||
}
|
||||
|
||||
private static void VerifyInputOrderDoesNotChangeResult()
|
||||
{
|
||||
var first = FleetLayoutCapture.Capture(
|
||||
new[]
|
||||
{
|
||||
new FleetMemberPose(
|
||||
1,
|
||||
new Pose2D(2.0, 3.0, 0.6)),
|
||||
new FleetMemberPose(
|
||||
2,
|
||||
new Pose2D(4.0, 5.0, -1.0))
|
||||
},
|
||||
leaderVehicleId: 1);
|
||||
var second = FleetLayoutCapture.Capture(
|
||||
new[]
|
||||
{
|
||||
new FleetMemberPose(
|
||||
2,
|
||||
new Pose2D(4.0, 5.0, -1.0)),
|
||||
new FleetMemberPose(
|
||||
1,
|
||||
new Pose2D(2.0, 3.0, 0.6))
|
||||
},
|
||||
leaderVehicleId: 1);
|
||||
|
||||
AssertPose(
|
||||
first.FleetPoseInWorld,
|
||||
second.FleetPoseInWorld,
|
||||
"输入顺序不变中心");
|
||||
AssertSameLayout(first.Layout, second.Layout);
|
||||
}
|
||||
|
||||
private static void VerifyEmptyInputIsRejected()
|
||||
{
|
||||
ExpectException<ArgumentException>(
|
||||
() => FleetLayoutCapture.Capture(
|
||||
Array.Empty<FleetMemberPose>(),
|
||||
leaderVehicleId: 1),
|
||||
"空成员集合");
|
||||
}
|
||||
|
||||
private static void VerifyDuplicateVehicleIdIsRejected()
|
||||
{
|
||||
ExpectException<ArgumentException>(
|
||||
() => FleetLayoutCapture.Capture(
|
||||
new[]
|
||||
{
|
||||
new FleetMemberPose(
|
||||
1,
|
||||
Pose2D.Identity),
|
||||
new FleetMemberPose(
|
||||
1,
|
||||
new Pose2D(1.0, 0.0, 0.0))
|
||||
},
|
||||
leaderVehicleId: 1),
|
||||
"重复车号");
|
||||
}
|
||||
|
||||
private static void VerifyMissingLeaderIsRejected()
|
||||
{
|
||||
ExpectException<ArgumentException>(
|
||||
() => FleetLayoutCapture.Capture(
|
||||
new[]
|
||||
{
|
||||
new FleetMemberPose(
|
||||
2,
|
||||
Pose2D.Identity)
|
||||
},
|
||||
leaderVehicleId: 1),
|
||||
"缺少主车");
|
||||
}
|
||||
|
||||
private static void AssertSameLayout(
|
||||
FleetLayout first,
|
||||
FleetLayout second)
|
||||
{
|
||||
if (first.VehicleCount != second.VehicleCount)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"输入顺序变化后成员数量发生变化。");
|
||||
}
|
||||
|
||||
for (var index = 0;
|
||||
index < first.Vehicles.Count;
|
||||
index++)
|
||||
{
|
||||
var vehicle = first.Vehicles[index];
|
||||
AssertVehicleLayout(
|
||||
second,
|
||||
vehicle.VehicleId,
|
||||
vehicle.PoseInFleet,
|
||||
"输入顺序不变布局");
|
||||
}
|
||||
}
|
||||
|
||||
private static void AssertVehicleLayout(
|
||||
FleetLayout layout,
|
||||
int vehicleId,
|
||||
Pose2D expectedPoseInFleet,
|
||||
string scenario)
|
||||
{
|
||||
if (!layout.TryGetVehicle(
|
||||
vehicleId,
|
||||
out var vehicle))
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"{scenario}缺少车辆{vehicleId}。");
|
||||
}
|
||||
|
||||
AssertPose(
|
||||
vehicle.PoseInFleet,
|
||||
expectedPoseInFleet,
|
||||
scenario);
|
||||
}
|
||||
|
||||
private static void AssertPose(
|
||||
Pose2D actual,
|
||||
Pose2D expected,
|
||||
string scenario)
|
||||
{
|
||||
AssertNear(
|
||||
actual.XMeters,
|
||||
expected.XMeters,
|
||||
scenario + " X");
|
||||
AssertNear(
|
||||
actual.YMeters,
|
||||
expected.YMeters,
|
||||
scenario + " Y");
|
||||
AssertNear(
|
||||
AngleMath.NormalizeRadians(
|
||||
actual.YawRadians -
|
||||
expected.YawRadians),
|
||||
0.0,
|
||||
scenario + " Yaw");
|
||||
}
|
||||
|
||||
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 static void ExpectException<TException>(
|
||||
Action action,
|
||||
string scenario)
|
||||
where TException : Exception
|
||||
{
|
||||
try
|
||||
{
|
||||
action();
|
||||
}
|
||||
catch (TException)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
throw new InvalidOperationException(
|
||||
$"{scenario}没有抛出{typeof(TException).Name}。");
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,302 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.Fleet;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Tests
|
||||
{
|
||||
internal static class FleetMemberCommandCorrectorTests
|
||||
{
|
||||
private const double Tolerance = 1e-9;
|
||||
|
||||
public static void Run()
|
||||
{
|
||||
VerifyZeroErrorsPreserveBaseCommands();
|
||||
VerifyRelativePositionErrorProducesOpposingCorrection();
|
||||
VerifyCommonTranslationIsRemoved();
|
||||
VerifyCommonRotationIsRemoved();
|
||||
VerifyDeadbandSuppressesSmallErrors();
|
||||
VerifyCorrectionLimits();
|
||||
|
||||
Console.WriteLine(
|
||||
"FleetMemberCommandCorrector测试通过，共6个场景。");
|
||||
}
|
||||
|
||||
private static void VerifyZeroErrorsPreserveBaseCommands()
|
||||
{
|
||||
var layout = CreateLayout();
|
||||
var baseCommands = FleetKinematics.Decompose(
|
||||
layout,
|
||||
new FleetMotionCommand(
|
||||
Point2D.Zero,
|
||||
new Twist2D(0.4, 0.1, 0.05)));
|
||||
var corrected = CreateCorrector().Correct(
|
||||
layout,
|
||||
baseCommands,
|
||||
CreateErrors(Pose2D.Identity, Pose2D.Identity));
|
||||
|
||||
AssertCommandsEqual(
|
||||
corrected,
|
||||
baseCommands,
|
||||
"零布局误差");
|
||||
}
|
||||
|
||||
private static void
|
||||
VerifyRelativePositionErrorProducesOpposingCorrection()
|
||||
{
|
||||
var layout = CreateLayout();
|
||||
var corrected = CreateCorrector().Correct(
|
||||
layout,
|
||||
CreateStopCommands(layout),
|
||||
CreateErrors(
|
||||
new Pose2D(0.02, 0.0, 0.0),
|
||||
new Pose2D(0.02, 0.0, 0.0)));
|
||||
|
||||
AssertTwist(
|
||||
FindCommand(corrected, 1).TwistInVehicleBody,
|
||||
-0.02,
|
||||
0.0,
|
||||
0.0,
|
||||
"车辆1相对位置纠偏");
|
||||
AssertTwist(
|
||||
FindCommand(corrected, 2).TwistInVehicleBody,
|
||||
-0.02,
|
||||
0.0,
|
||||
0.0,
|
||||
"车辆2相对位置纠偏");
|
||||
}
|
||||
|
||||
private static void VerifyCommonTranslationIsRemoved()
|
||||
{
|
||||
var layout = CreateLayout();
|
||||
var corrected = CreateCorrector().Correct(
|
||||
layout,
|
||||
CreateStopCommands(layout),
|
||||
CreateErrors(
|
||||
new Pose2D(0.02, 0.0, 0.0),
|
||||
new Pose2D(-0.02, 0.0, 0.0)));
|
||||
|
||||
AssertAllStopped(
|
||||
corrected,
|
||||
"共同平移不应成为成员相对纠偏");
|
||||
}
|
||||
|
||||
private static void VerifyCommonRotationIsRemoved()
|
||||
{
|
||||
const double fleetYawErrorRadians = 0.02;
|
||||
var layout = CreateLayout();
|
||||
var commonRotationError = new Pose2D(
|
||||
0.0,
|
||||
-fleetYawErrorRadians,
|
||||
fleetYawErrorRadians);
|
||||
var corrected = CreateCorrector().Correct(
|
||||
layout,
|
||||
CreateStopCommands(layout),
|
||||
CreateErrors(
|
||||
commonRotationError,
|
||||
commonRotationError));
|
||||
|
||||
AssertAllStopped(
|
||||
corrected,
|
||||
"共同旋转不应成为成员相对纠偏");
|
||||
}
|
||||
|
||||
private static void VerifyDeadbandSuppressesSmallErrors()
|
||||
{
|
||||
var layout = CreateLayout();
|
||||
var corrector = new FleetMemberCommandCorrector(
|
||||
longitudinalPositionGainPerSecond: 1.0,
|
||||
lateralPositionGainPerSecond: 1.0,
|
||||
yawGainPerSecond: 1.0,
|
||||
positionErrorDeadbandMeters: 0.005,
|
||||
yawErrorDeadbandRadians: 0.02,
|
||||
maximumLinearCorrectionMetersPerSecond: 1.0,
|
||||
maximumAngularCorrectionRadiansPerSecond: 1.0);
|
||||
var corrected = corrector.Correct(
|
||||
layout,
|
||||
CreateStopCommands(layout),
|
||||
CreateErrors(
|
||||
new Pose2D(0.004, 0.003, 0.01),
|
||||
new Pose2D(0.004, -0.003, -0.01)));
|
||||
|
||||
AssertAllStopped(corrected, "布局误差死区");
|
||||
}
|
||||
|
||||
private static void VerifyCorrectionLimits()
|
||||
{
|
||||
var layout = CreateLayout();
|
||||
var corrector = new FleetMemberCommandCorrector(
|
||||
longitudinalPositionGainPerSecond: 1.0,
|
||||
lateralPositionGainPerSecond: 1.0,
|
||||
yawGainPerSecond: 1.0,
|
||||
positionErrorDeadbandMeters: 0.0,
|
||||
yawErrorDeadbandRadians: 0.0,
|
||||
maximumLinearCorrectionMetersPerSecond: 0.03,
|
||||
maximumAngularCorrectionRadiansPerSecond: 0.05);
|
||||
var corrected = corrector.Correct(
|
||||
layout,
|
||||
CreateStopCommands(layout),
|
||||
CreateErrors(
|
||||
new Pose2D(0.2, 0.0, 0.2),
|
||||
new Pose2D(0.2, 0.0, -0.2)));
|
||||
|
||||
for (var index = 0; index < corrected.Count; index++)
|
||||
{
|
||||
var twist = corrected[index].TwistInVehicleBody;
|
||||
var linearMagnitude = Math.Sqrt(
|
||||
twist.VxMetersPerSecond *
|
||||
twist.VxMetersPerSecond +
|
||||
twist.VyMetersPerSecond *
|
||||
twist.VyMetersPerSecond);
|
||||
|
||||
AssertNear(
|
||||
linearMagnitude,
|
||||
0.03,
|
||||
"线速度纠偏限幅");
|
||||
AssertNear(
|
||||
Math.Abs(twist.OmegaRadiansPerSecond),
|
||||
0.05,
|
||||
"角速度纠偏限幅");
|
||||
}
|
||||
}
|
||||
|
||||
private static FleetMemberCommandCorrector CreateCorrector()
|
||||
{
|
||||
return new FleetMemberCommandCorrector(
|
||||
longitudinalPositionGainPerSecond: 1.0,
|
||||
lateralPositionGainPerSecond: 1.0,
|
||||
yawGainPerSecond: 1.0,
|
||||
positionErrorDeadbandMeters: 0.0,
|
||||
yawErrorDeadbandRadians: 0.0,
|
||||
maximumLinearCorrectionMetersPerSecond: 1.0,
|
||||
maximumAngularCorrectionRadiansPerSecond: 1.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 IReadOnlyList<FleetMemberCommand>
|
||||
CreateStopCommands(FleetLayout layout)
|
||||
{
|
||||
return FleetKinematics.Decompose(
|
||||
layout,
|
||||
FleetMotionCommand.Stop());
|
||||
}
|
||||
|
||||
private static FleetMemberLayoutError[] CreateErrors(
|
||||
Pose2D vehicle1Error,
|
||||
Pose2D vehicle2Error)
|
||||
{
|
||||
return new[]
|
||||
{
|
||||
new FleetMemberLayoutError(1, vehicle1Error),
|
||||
new FleetMemberLayoutError(2, vehicle2Error)
|
||||
};
|
||||
}
|
||||
|
||||
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 AssertCommandsEqual(
|
||||
IReadOnlyList<FleetMemberCommand> actual,
|
||||
IReadOnlyList<FleetMemberCommand> expected,
|
||||
string scenario)
|
||||
{
|
||||
if (actual.Count != expected.Count)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"{scenario}的命令数量不一致。");
|
||||
}
|
||||
|
||||
for (var index = 0; index < expected.Count; index++)
|
||||
{
|
||||
var expectedCommand = expected[index];
|
||||
var actualCommand = FindCommand(
|
||||
actual,
|
||||
expectedCommand.VehicleId);
|
||||
AssertTwist(
|
||||
actualCommand.TwistInVehicleBody,
|
||||
expectedCommand.TwistInVehicleBody
|
||||
.VxMetersPerSecond,
|
||||
expectedCommand.TwistInVehicleBody
|
||||
.VyMetersPerSecond,
|
||||
expectedCommand.TwistInVehicleBody
|
||||
.OmegaRadiansPerSecond,
|
||||
scenario);
|
||||
}
|
||||
}
|
||||
|
||||
private static void AssertAllStopped(
|
||||
IReadOnlyList<FleetMemberCommand> commands,
|
||||
string scenario)
|
||||
{
|
||||
for (var index = 0; index < commands.Count; index++)
|
||||
{
|
||||
AssertTwist(
|
||||
commands[index].TwistInVehicleBody,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
scenario);
|
||||
}
|
||||
}
|
||||
|
||||
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}。");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,310 @@
|
||||
using System;
|
||||
using MultiWheelC.Fleet;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Tests
|
||||
{
|
||||
internal static class FleetPreparationCoordinatorTests
|
||||
{
|
||||
private const double Tolerance = 1e-9;
|
||||
|
||||
public static void Run()
|
||||
{
|
||||
VerifyLeaderYawExample();
|
||||
VerifyTailToTailUsesEquivalentAxis();
|
||||
VerifyAllMembersMustBeReady();
|
||||
VerifyStaleStatusIsIgnored();
|
||||
VerifyMemberFaultIsLatched();
|
||||
VerifyFaultAfterAuthorizationIsLatched();
|
||||
VerifyPrematureActiveIsRejected();
|
||||
VerifyCancelClearsPlan();
|
||||
|
||||
Console.WriteLine(
|
||||
"FleetPreparationCoordinator测试通过。共8个场景。");
|
||||
}
|
||||
|
||||
private static void VerifyLeaderYawExample()
|
||||
{
|
||||
var capture = FleetLayoutCapture.Capture(
|
||||
new[]
|
||||
{
|
||||
new FleetMemberPose(
|
||||
1,
|
||||
new Pose2D(
|
||||
-1.0,
|
||||
0.0,
|
||||
AngleMath.DegreesToRadians(20.0))),
|
||||
new FleetMemberPose(
|
||||
2,
|
||||
new Pose2D(1.0, 0.0, 0.0))
|
||||
},
|
||||
leaderVehicleId: 1);
|
||||
var coordinator =
|
||||
new FleetPreparationCoordinator();
|
||||
|
||||
coordinator.StartRollingPreparation(
|
||||
planId: 1,
|
||||
capture.Layout,
|
||||
motionDirectionInFleetRadians: 0.0);
|
||||
|
||||
AssertTargetDegrees(coordinator, 1, 0.0);
|
||||
AssertTargetDegrees(coordinator, 2, 20.0);
|
||||
}
|
||||
|
||||
private static void VerifyTailToTailUsesEquivalentAxis()
|
||||
{
|
||||
var coordinator =
|
||||
new FleetPreparationCoordinator();
|
||||
coordinator.StartRollingPreparation(
|
||||
planId: 2,
|
||||
CreateTailToTailLayout(),
|
||||
motionDirectionInFleetRadians: 0.0);
|
||||
|
||||
AssertTargetDegrees(coordinator, 1, 0.0);
|
||||
AssertTargetDegrees(coordinator, 2, 0.0);
|
||||
}
|
||||
|
||||
private static void VerifyAllMembersMustBeReady()
|
||||
{
|
||||
var coordinator = CreateStartedCoordinator(3);
|
||||
|
||||
AssertState(
|
||||
coordinator.ReportMemberStatus(
|
||||
CreateStatus(
|
||||
3,
|
||||
1,
|
||||
FleetMemberAgentState.Ready)),
|
||||
FleetPreparationCoordinatorState
|
||||
.WaitingForMembers,
|
||||
"仅一辆车Ready");
|
||||
AssertState(
|
||||
coordinator.ReportMemberStatus(
|
||||
CreateStatus(
|
||||
3,
|
||||
2,
|
||||
FleetMemberAgentState.Ready)),
|
||||
FleetPreparationCoordinatorState
|
||||
.ReadyToActivate,
|
||||
"全部成员Ready");
|
||||
AssertState(
|
||||
coordinator.ReportMemberStatus(
|
||||
CreateStatus(
|
||||
3,
|
||||
1,
|
||||
FleetMemberAgentState.Preparing)),
|
||||
FleetPreparationCoordinatorState
|
||||
.WaitingForMembers,
|
||||
"成员失去Ready");
|
||||
AssertState(
|
||||
coordinator.ReportMemberStatus(
|
||||
CreateStatus(
|
||||
3,
|
||||
1,
|
||||
FleetMemberAgentState.Ready)),
|
||||
FleetPreparationCoordinatorState
|
||||
.ReadyToActivate,
|
||||
"成员重新Ready");
|
||||
|
||||
if (coordinator.TryAuthorizeActivation(4))
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"错误任务编号不应获得激活授权。");
|
||||
}
|
||||
|
||||
if (!coordinator.TryAuthorizeActivation(3))
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"全部成员Ready后没有获得激活授权。");
|
||||
}
|
||||
|
||||
AssertState(
|
||||
coordinator.State,
|
||||
FleetPreparationCoordinatorState
|
||||
.ActivationAuthorized,
|
||||
"统一激活授权");
|
||||
}
|
||||
|
||||
private static void VerifyStaleStatusIsIgnored()
|
||||
{
|
||||
var coordinator = CreateStartedCoordinator(5);
|
||||
var state = coordinator.ReportMemberStatus(
|
||||
CreateStatus(
|
||||
4,
|
||||
1,
|
||||
FleetMemberAgentState.Ready));
|
||||
|
||||
AssertState(
|
||||
state,
|
||||
FleetPreparationCoordinatorState
|
||||
.WaitingForMembers,
|
||||
"旧任务状态报告");
|
||||
}
|
||||
|
||||
private static void VerifyMemberFaultIsLatched()
|
||||
{
|
||||
var coordinator = CreateStartedCoordinator(6);
|
||||
var state = coordinator.ReportMemberStatus(
|
||||
CreateStatus(
|
||||
6,
|
||||
2,
|
||||
FleetMemberAgentState.Faulted,
|
||||
"舵轮未到位"));
|
||||
|
||||
AssertState(
|
||||
state,
|
||||
FleetPreparationCoordinatorState.Faulted,
|
||||
"成员准备故障");
|
||||
if (string.IsNullOrWhiteSpace(
|
||||
coordinator.LastFailureReason))
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"成员准备故障没有保存原因。");
|
||||
}
|
||||
}
|
||||
|
||||
private static void VerifyFaultAfterAuthorizationIsLatched()
|
||||
{
|
||||
var coordinator = CreateStartedCoordinator(9);
|
||||
coordinator.ReportMemberStatus(
|
||||
CreateStatus(
|
||||
9,
|
||||
1,
|
||||
FleetMemberAgentState.Ready));
|
||||
coordinator.ReportMemberStatus(
|
||||
CreateStatus(
|
||||
9,
|
||||
2,
|
||||
FleetMemberAgentState.Ready));
|
||||
coordinator.TryAuthorizeActivation(9);
|
||||
|
||||
var state = coordinator.ReportMemberStatus(
|
||||
CreateStatus(
|
||||
9,
|
||||
2,
|
||||
FleetMemberAgentState.Faulted,
|
||||
"激活失败"));
|
||||
|
||||
AssertState(
|
||||
state,
|
||||
FleetPreparationCoordinatorState.Faulted,
|
||||
"授权后的成员故障");
|
||||
}
|
||||
|
||||
private static void VerifyPrematureActiveIsRejected()
|
||||
{
|
||||
var coordinator = CreateStartedCoordinator(7);
|
||||
var state = coordinator.ReportMemberStatus(
|
||||
CreateStatus(
|
||||
7,
|
||||
1,
|
||||
FleetMemberAgentState.Active));
|
||||
|
||||
AssertState(
|
||||
state,
|
||||
FleetPreparationCoordinatorState.Faulted,
|
||||
"成员提前运动");
|
||||
}
|
||||
|
||||
private static void VerifyCancelClearsPlan()
|
||||
{
|
||||
var coordinator = CreateStartedCoordinator(8);
|
||||
coordinator.Cancel();
|
||||
|
||||
AssertState(
|
||||
coordinator.State,
|
||||
FleetPreparationCoordinatorState.Idle,
|
||||
"取消准备任务");
|
||||
if (coordinator.CurrentPlanId != 0 ||
|
||||
coordinator.Targets.Count != 0)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"取消后没有清除准备任务数据。");
|
||||
}
|
||||
}
|
||||
|
||||
private static FleetPreparationCoordinator
|
||||
CreateStartedCoordinator(long planId)
|
||||
{
|
||||
var coordinator =
|
||||
new FleetPreparationCoordinator();
|
||||
coordinator.StartRollingPreparation(
|
||||
planId,
|
||||
CreateTailToTailLayout(),
|
||||
motionDirectionInFleetRadians: 0.0);
|
||||
return coordinator;
|
||||
}
|
||||
|
||||
private static FleetLayout CreateTailToTailLayout()
|
||||
{
|
||||
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 FleetMemberPreparationStatus CreateStatus(
|
||||
long planId,
|
||||
int vehicleId,
|
||||
FleetMemberAgentState state,
|
||||
string failureReason = "")
|
||||
{
|
||||
return new FleetMemberPreparationStatus(
|
||||
planId,
|
||||
vehicleId,
|
||||
state,
|
||||
failureReason);
|
||||
}
|
||||
|
||||
private static void AssertTargetDegrees(
|
||||
FleetPreparationCoordinator coordinator,
|
||||
int vehicleId,
|
||||
double expectedDegrees)
|
||||
{
|
||||
if (!coordinator.TryGetTarget(
|
||||
vehicleId,
|
||||
out var target))
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"没有找到车辆{vehicleId}的准备目标。");
|
||||
}
|
||||
|
||||
AssertNear(
|
||||
AngleMath.RadiansToDegrees(
|
||||
target.MotionDirectionInBodyRadians),
|
||||
expectedDegrees,
|
||||
$"车辆{vehicleId}本地β");
|
||||
}
|
||||
|
||||
private static void AssertState(
|
||||
FleetPreparationCoordinatorState actual,
|
||||
FleetPreparationCoordinatorState expected,
|
||||
string scenario)
|
||||
{
|
||||
if (actual != expected)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"{scenario}状态错误：" +
|
||||
$"actual={actual}, expected={expected}。");
|
||||
}
|
||||
}
|
||||
|
||||
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}。");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,430 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.Fleet;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Tests
|
||||
{
|
||||
internal static class FleetStateEstimatorTests
|
||||
{
|
||||
private const double Tolerance = 1e-9;
|
||||
|
||||
public static void Run()
|
||||
{
|
||||
VerifyRigidStateIsRecovered();
|
||||
VerifyOlderSamplesAreAligned();
|
||||
VerifyYawWrapAroundIsAveraged();
|
||||
VerifySmallLayoutErrorIsReported();
|
||||
VerifyInconsistentCentersAreRejected();
|
||||
VerifyMissingMemberIsRejected();
|
||||
VerifyInvalidVelocityRemainsExplicit();
|
||||
|
||||
Console.WriteLine(
|
||||
"FleetStateEstimator车队状态估计测试通过。共7个场景。");
|
||||
}
|
||||
|
||||
private static void VerifyRigidStateIsRecovered()
|
||||
{
|
||||
var layout = CreateLayout();
|
||||
var fleetPose = new Pose2D(4.0, -2.0, 0.4);
|
||||
var fleetTwist = new Twist2D(0.3, -0.1, 0.2);
|
||||
var result = CreateEstimator().Estimate(
|
||||
layout,
|
||||
CreateRigidMemberStates(
|
||||
layout,
|
||||
fleetPose,
|
||||
fleetTwist,
|
||||
sampleTimestampSeconds: 5.0,
|
||||
hasValidVelocityEstimate: true),
|
||||
targetTimestampSeconds: 5.0);
|
||||
|
||||
var state = RequireState(result, "刚体状态还原");
|
||||
AssertPose(
|
||||
state.FleetPoseInWorld,
|
||||
fleetPose,
|
||||
"刚体状态还原");
|
||||
AssertTwist(
|
||||
state.TwistAtFleetOriginInWorld,
|
||||
fleetTwist,
|
||||
"刚体速度还原");
|
||||
|
||||
for (var index = 0;
|
||||
index < result.MemberErrors.Count;
|
||||
index++)
|
||||
{
|
||||
AssertPose(
|
||||
result.MemberErrors[index]
|
||||
.ActualPoseInExpectedVehicleFrame,
|
||||
Pose2D.Identity,
|
||||
"刚体布局误差");
|
||||
}
|
||||
}
|
||||
|
||||
private static void VerifyOlderSamplesAreAligned()
|
||||
{
|
||||
var layout = CreateLayout();
|
||||
var sampleFleetPose =
|
||||
new Pose2D(1.0, 2.0, 0.3);
|
||||
var fleetTwist =
|
||||
new Twist2D(0.4, -0.2, 0.0);
|
||||
var result = CreateEstimator().Estimate(
|
||||
layout,
|
||||
CreateRigidMemberStates(
|
||||
layout,
|
||||
sampleFleetPose,
|
||||
fleetTwist,
|
||||
sampleTimestampSeconds: 0.9,
|
||||
hasValidVelocityEstimate: true),
|
||||
targetTimestampSeconds: 1.0);
|
||||
|
||||
var state = RequireState(result, "成员时间对齐");
|
||||
AssertPose(
|
||||
state.FleetPoseInWorld,
|
||||
new Pose2D(1.04, 1.98, 0.3),
|
||||
"成员时间对齐");
|
||||
AssertTwist(
|
||||
state.TwistAtFleetOriginInWorld,
|
||||
fleetTwist,
|
||||
"时间对齐后速度");
|
||||
}
|
||||
|
||||
private static void VerifySmallLayoutErrorIsReported()
|
||||
{
|
||||
var layout = CreateLayout();
|
||||
var result = CreateEstimator().Estimate(
|
||||
layout,
|
||||
new[]
|
||||
{
|
||||
CreateMemberState(
|
||||
1,
|
||||
new Pose2D(-0.98, 0.0, 0.0),
|
||||
Twist2D.Zero,
|
||||
1.0,
|
||||
true),
|
||||
CreateMemberState(
|
||||
2,
|
||||
new Pose2D(0.99, 0.0, Math.PI),
|
||||
Twist2D.Zero,
|
||||
1.0,
|
||||
true)
|
||||
},
|
||||
targetTimestampSeconds: 1.0);
|
||||
|
||||
var state = RequireState(result, "小范围布局误差");
|
||||
AssertNear(
|
||||
state.FleetPoseInWorld.XMeters,
|
||||
0.005,
|
||||
"小范围布局误差中心X");
|
||||
AssertNear(
|
||||
FindError(result.MemberErrors, 1)
|
||||
.ActualPoseInExpectedVehicleFrame.XMeters,
|
||||
0.015,
|
||||
"车辆1布局误差X");
|
||||
AssertNear(
|
||||
FindError(result.MemberErrors, 2)
|
||||
.ActualPoseInExpectedVehicleFrame.XMeters,
|
||||
0.015,
|
||||
"车辆2布局误差X");
|
||||
}
|
||||
|
||||
private static void VerifyYawWrapAroundIsAveraged()
|
||||
{
|
||||
var layout = CreateLayout();
|
||||
var firstCandidate = new Pose2D(
|
||||
0.0,
|
||||
0.0,
|
||||
AngleMath.DegreesToRadians(179.0));
|
||||
var secondCandidate = new Pose2D(
|
||||
0.0,
|
||||
0.0,
|
||||
AngleMath.DegreesToRadians(-179.0));
|
||||
var result = CreateEstimator().Estimate(
|
||||
layout,
|
||||
new[]
|
||||
{
|
||||
CreateMemberState(
|
||||
1,
|
||||
FrameTransform2D.Compose(
|
||||
firstCandidate,
|
||||
layout.Vehicles[0].PoseInFleet),
|
||||
Twist2D.Zero,
|
||||
1.0,
|
||||
true),
|
||||
CreateMemberState(
|
||||
2,
|
||||
FrameTransform2D.Compose(
|
||||
secondCandidate,
|
||||
layout.Vehicles[1].PoseInFleet),
|
||||
Twist2D.Zero,
|
||||
1.0,
|
||||
true)
|
||||
},
|
||||
targetTimestampSeconds: 1.0);
|
||||
|
||||
var state = RequireState(result, "跨正负π航向平均");
|
||||
AssertNear(
|
||||
Math.Abs(state.FleetPoseInWorld.YawRadians),
|
||||
Math.PI,
|
||||
"跨正负π航向平均");
|
||||
}
|
||||
|
||||
private static void VerifyInconsistentCentersAreRejected()
|
||||
{
|
||||
var result = CreateEstimator().Estimate(
|
||||
CreateLayout(),
|
||||
new[]
|
||||
{
|
||||
CreateMemberState(
|
||||
1,
|
||||
new Pose2D(-1.0, 0.0, 0.0),
|
||||
Twist2D.Zero,
|
||||
1.0,
|
||||
true),
|
||||
CreateMemberState(
|
||||
2,
|
||||
new Pose2D(1.3, 0.0, Math.PI),
|
||||
Twist2D.Zero,
|
||||
1.0,
|
||||
true)
|
||||
},
|
||||
targetTimestampSeconds: 1.0);
|
||||
|
||||
AssertUnavailable(result, "候选中心冲突");
|
||||
}
|
||||
|
||||
private static void VerifyMissingMemberIsRejected()
|
||||
{
|
||||
var result = CreateEstimator().Estimate(
|
||||
CreateLayout(),
|
||||
new[]
|
||||
{
|
||||
CreateMemberState(
|
||||
1,
|
||||
new Pose2D(-1.0, 0.0, 0.0),
|
||||
Twist2D.Zero,
|
||||
1.0,
|
||||
true)
|
||||
},
|
||||
targetTimestampSeconds: 1.0);
|
||||
|
||||
AssertUnavailable(result, "成员缺失");
|
||||
}
|
||||
|
||||
private static void VerifyInvalidVelocityRemainsExplicit()
|
||||
{
|
||||
var layout = CreateLayout();
|
||||
var result = CreateEstimator().Estimate(
|
||||
layout,
|
||||
CreateRigidMemberStates(
|
||||
layout,
|
||||
Pose2D.Identity,
|
||||
new Twist2D(0.4, 0.0, 0.0),
|
||||
sampleTimestampSeconds: 1.0,
|
||||
hasValidVelocityEstimate: false),
|
||||
targetTimestampSeconds: 1.0);
|
||||
|
||||
var state = RequireState(result, "速度未初始化");
|
||||
if (state.HasValidVelocityEstimate)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"成员速度无效时车队速度不应标记为有效。");
|
||||
}
|
||||
|
||||
AssertTwist(
|
||||
state.TwistAtFleetOriginInWorld,
|
||||
Twist2D.Zero,
|
||||
"速度未初始化");
|
||||
}
|
||||
|
||||
private static FleetStateEstimator CreateEstimator()
|
||||
{
|
||||
return new FleetStateEstimator(
|
||||
maximumMemberStateAgeSeconds: 0.25,
|
||||
maximumPositionDisagreementMeters: 0.1,
|
||||
maximumYawDisagreementRadians:
|
||||
AngleMath.DegreesToRadians(5.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 FleetMemberStateSample[]
|
||||
CreateRigidMemberStates(
|
||||
FleetLayout layout,
|
||||
Pose2D fleetPoseInWorld,
|
||||
Twist2D twistAtFleetOriginInWorld,
|
||||
double sampleTimestampSeconds,
|
||||
bool hasValidVelocityEstimate)
|
||||
{
|
||||
var states =
|
||||
new FleetMemberStateSample[layout.VehicleCount];
|
||||
|
||||
for (var index = 0;
|
||||
index < layout.Vehicles.Count;
|
||||
index++)
|
||||
{
|
||||
var vehicle = layout.Vehicles[index];
|
||||
var memberPoseInWorld =
|
||||
FrameTransform2D.Compose(
|
||||
fleetPoseInWorld,
|
||||
vehicle.PoseInFleet);
|
||||
var xFromFleetOrigin =
|
||||
memberPoseInWorld.XMeters -
|
||||
fleetPoseInWorld.XMeters;
|
||||
var yFromFleetOrigin =
|
||||
memberPoseInWorld.YMeters -
|
||||
fleetPoseInWorld.YMeters;
|
||||
var memberTwistInWorld = new Twist2D(
|
||||
twistAtFleetOriginInWorld
|
||||
.VxMetersPerSecond -
|
||||
twistAtFleetOriginInWorld
|
||||
.OmegaRadiansPerSecond *
|
||||
yFromFleetOrigin,
|
||||
twistAtFleetOriginInWorld
|
||||
.VyMetersPerSecond +
|
||||
twistAtFleetOriginInWorld
|
||||
.OmegaRadiansPerSecond *
|
||||
xFromFleetOrigin,
|
||||
twistAtFleetOriginInWorld
|
||||
.OmegaRadiansPerSecond);
|
||||
|
||||
states[index] = new FleetMemberStateSample(
|
||||
vehicle.VehicleId,
|
||||
sampleTimestampSeconds,
|
||||
memberPoseInWorld,
|
||||
memberTwistInWorld,
|
||||
isStateAvailable: true,
|
||||
hasValidVelocityEstimate:
|
||||
hasValidVelocityEstimate);
|
||||
}
|
||||
|
||||
return states;
|
||||
}
|
||||
|
||||
private static FleetMemberStateSample CreateMemberState(
|
||||
int vehicleId,
|
||||
Pose2D poseInWorld,
|
||||
Twist2D twistInWorld,
|
||||
double timestampSeconds,
|
||||
bool hasValidVelocityEstimate)
|
||||
{
|
||||
return new FleetMemberStateSample(
|
||||
vehicleId,
|
||||
timestampSeconds,
|
||||
poseInWorld,
|
||||
twistInWorld,
|
||||
isStateAvailable: true,
|
||||
hasValidVelocityEstimate:
|
||||
hasValidVelocityEstimate);
|
||||
}
|
||||
|
||||
private static FleetState RequireState(
|
||||
FleetStateEstimateResult result,
|
||||
string scenario)
|
||||
{
|
||||
if (!result.IsAvailable || !result.State.HasValue)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"{scenario}应产生可用状态：" +
|
||||
result.UnavailableReason);
|
||||
}
|
||||
|
||||
return result.State.Value;
|
||||
}
|
||||
|
||||
private static void AssertUnavailable(
|
||||
FleetStateEstimateResult result,
|
||||
string scenario)
|
||||
{
|
||||
if (result.IsAvailable ||
|
||||
string.IsNullOrWhiteSpace(
|
||||
result.UnavailableReason))
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"{scenario}应返回带原因的不可用结果。");
|
||||
}
|
||||
}
|
||||
|
||||
private static FleetMemberLayoutError FindError(
|
||||
IReadOnlyList<FleetMemberLayoutError> errors,
|
||||
int vehicleId)
|
||||
{
|
||||
for (var index = 0;
|
||||
index < errors.Count;
|
||||
index++)
|
||||
{
|
||||
if (errors[index].VehicleId == vehicleId)
|
||||
{
|
||||
return errors[index];
|
||||
}
|
||||
}
|
||||
|
||||
throw new InvalidOperationException(
|
||||
$"没有找到车辆{vehicleId}的布局误差。");
|
||||
}
|
||||
|
||||
private static void AssertPose(
|
||||
Pose2D actual,
|
||||
Pose2D expected,
|
||||
string scenario)
|
||||
{
|
||||
AssertNear(
|
||||
actual.XMeters,
|
||||
expected.XMeters,
|
||||
scenario + " X");
|
||||
AssertNear(
|
||||
actual.YMeters,
|
||||
expected.YMeters,
|
||||
scenario + " Y");
|
||||
AssertNear(
|
||||
AngleMath.ShortestDifferenceRadians(
|
||||
actual.YawRadians,
|
||||
expected.YawRadians),
|
||||
0.0,
|
||||
scenario + " Yaw");
|
||||
}
|
||||
|
||||
private static void AssertTwist(
|
||||
Twist2D actual,
|
||||
Twist2D expected,
|
||||
string scenario)
|
||||
{
|
||||
AssertNear(
|
||||
actual.VxMetersPerSecond,
|
||||
expected.VxMetersPerSecond,
|
||||
scenario + " Vx");
|
||||
AssertNear(
|
||||
actual.VyMetersPerSecond,
|
||||
expected.VyMetersPerSecond,
|
||||
scenario + " Vy");
|
||||
AssertNear(
|
||||
actual.OmegaRadiansPerSecond,
|
||||
expected.OmegaRadiansPerSecond,
|
||||
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}。");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -35,8 +35,13 @@ namespace MultiWheelC.Tests
|
||||
|
||||
Console.WriteLine(
|
||||
"Stanley前进/倒车横向符号测试通过。共8个场景。");
|
||||
FleetLayoutCaptureTests.Run();
|
||||
FleetStateEstimatorTests.Run();
|
||||
FleetKinematicsTests.Run();
|
||||
FleetControllerTests.Run();
|
||||
FleetMemberCommandCorrectorTests.Run();
|
||||
FleetCoordinatorTests.Run();
|
||||
FleetPreparationCoordinatorTests.Run();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
|
||||
Reference in New Issue
Block a user