完善状态估计、车队协调与舵轮辨识日志
This commit is contained in:
@@ -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}。");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user