贯通车队运行链并支持轨迹自动推导β
This commit is contained in:
@@ -0,0 +1,240 @@
|
||||
using System;
|
||||
using System.Numerics;
|
||||
using CommonUsage.Chassis;
|
||||
using MultiWheelC.Fleet;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Tests
|
||||
{
|
||||
internal static class FleetMemberAgentTests
|
||||
{
|
||||
private const long PlanId = 11;
|
||||
private const int VehicleId = 2;
|
||||
|
||||
public static void Run()
|
||||
{
|
||||
VerifyActiveWatchdogExpires();
|
||||
VerifyValidMotionCommandRefreshesDeadline();
|
||||
VerifyRepeatedActivationDoesNotRefreshDeadline();
|
||||
VerifyLateActivationCannotResumeMotion();
|
||||
VerifyStopClearsWatchdog();
|
||||
|
||||
Console.WriteLine(
|
||||
"FleetMemberAgent本地命令看门狗测试通过，共5个场景。");
|
||||
}
|
||||
|
||||
private static void VerifyActiveWatchdogExpires()
|
||||
{
|
||||
var agent = CreateReadyAgent();
|
||||
AssertTrue(
|
||||
agent.Activate(
|
||||
PlanId,
|
||||
commandReceivedTimeSeconds: 10.0,
|
||||
validForSeconds: 0.5),
|
||||
"成员车应当成功激活");
|
||||
|
||||
AssertTrue(
|
||||
agent.UpdateCommandWatchdog(10.5),
|
||||
"截止时刻仍应视为有效");
|
||||
AssertFalse(
|
||||
agent.UpdateCommandWatchdog(10.501),
|
||||
"超过命令有效期后应停车");
|
||||
AssertState(
|
||||
agent,
|
||||
FleetMemberAgentState.Faulted,
|
||||
"命令超时");
|
||||
AssertTrue(
|
||||
!string.IsNullOrWhiteSpace(
|
||||
agent.LastFailureReason),
|
||||
"命令超时应保留故障原因");
|
||||
}
|
||||
|
||||
private static void VerifyValidMotionCommandRefreshesDeadline()
|
||||
{
|
||||
var agent = CreateReadyAgent();
|
||||
agent.Activate(
|
||||
PlanId,
|
||||
commandReceivedTimeSeconds: 20.0,
|
||||
validForSeconds: 0.5);
|
||||
|
||||
var accepted = agent.Execute(
|
||||
PlanId,
|
||||
new FleetMemberCommand(
|
||||
VehicleId,
|
||||
Twist2D.Zero),
|
||||
commandReceivedTimeSeconds: 20.4,
|
||||
validForSeconds: 0.7);
|
||||
|
||||
AssertTrue(
|
||||
accepted,
|
||||
"有效速度命令应被接受");
|
||||
AssertNear(
|
||||
agent.LastAcceptedCommandTimeSeconds,
|
||||
20.4,
|
||||
"最近命令接收时间");
|
||||
AssertNear(
|
||||
agent.CommandDeadlineSeconds,
|
||||
21.1,
|
||||
"速度命令刷新后的截止时间");
|
||||
AssertTrue(
|
||||
agent.UpdateCommandWatchdog(20.8),
|
||||
"刷新截止时间后车辆应保持Active");
|
||||
}
|
||||
|
||||
private static void VerifyRepeatedActivationDoesNotRefreshDeadline()
|
||||
{
|
||||
var agent = CreateReadyAgent();
|
||||
agent.Activate(
|
||||
PlanId,
|
||||
commandReceivedTimeSeconds: 30.0,
|
||||
validForSeconds: 0.5);
|
||||
|
||||
AssertTrue(
|
||||
agent.Activate(
|
||||
PlanId,
|
||||
commandReceivedTimeSeconds: 30.4,
|
||||
validForSeconds: 0.5),
|
||||
"未超时的重复激活命令应允许幂等确认");
|
||||
AssertNear(
|
||||
agent.CommandDeadlineSeconds,
|
||||
30.5,
|
||||
"重复激活不能替代运动命令刷新截止时间");
|
||||
AssertFalse(
|
||||
agent.UpdateCommandWatchdog(30.501),
|
||||
"没有收到运动命令时仍应按最初激活期限停车");
|
||||
}
|
||||
|
||||
private static void VerifyLateActivationCannotResumeMotion()
|
||||
{
|
||||
var agent = CreateReadyAgent();
|
||||
agent.Activate(
|
||||
PlanId,
|
||||
commandReceivedTimeSeconds: 40.0,
|
||||
validForSeconds: 0.5);
|
||||
|
||||
AssertFalse(
|
||||
agent.Activate(
|
||||
PlanId,
|
||||
commandReceivedTimeSeconds: 40.6,
|
||||
validForSeconds: 0.5),
|
||||
"迟到的激活命令不能恢复已经失联的车辆");
|
||||
AssertState(
|
||||
agent,
|
||||
FleetMemberAgentState.Faulted,
|
||||
"迟到激活命令");
|
||||
}
|
||||
|
||||
private static void VerifyStopClearsWatchdog()
|
||||
{
|
||||
var agent = CreateReadyAgent();
|
||||
agent.Activate(
|
||||
PlanId,
|
||||
commandReceivedTimeSeconds: 50.0,
|
||||
validForSeconds: 0.5);
|
||||
|
||||
agent.Stop();
|
||||
|
||||
AssertState(
|
||||
agent,
|
||||
FleetMemberAgentState.Idle,
|
||||
"正常停止");
|
||||
AssertTrue(
|
||||
!agent.LastAcceptedCommandTimeSeconds.HasValue &&
|
||||
!agent.CommandDeadlineSeconds.HasValue,
|
||||
"正常停止后应清除命令看门狗");
|
||||
}
|
||||
|
||||
private static FleetMemberAgent CreateReadyAgent()
|
||||
{
|
||||
var chassis = new MultiWheelChassis();
|
||||
chassis.AddWheel(CreateWheel(-500f, 300f));
|
||||
chassis.AddWheel(CreateWheel(-500f, -300f));
|
||||
chassis.AddWheel(CreateWheel(500f, 300f));
|
||||
chassis.AddWheel(CreateWheel(500f, -300f));
|
||||
chassis.Initialize();
|
||||
|
||||
var agent = new FleetMemberAgent(
|
||||
new MultiWheelChassisAdapter(
|
||||
chassis,
|
||||
VehicleId),
|
||||
alignmentToleranceRadians:
|
||||
AngleMath.DegreesToRadians(1.0),
|
||||
alignmentStableSeconds: 0.0);
|
||||
|
||||
AssertTrue(
|
||||
agent.BeginRollingPreparation(
|
||||
PlanId,
|
||||
motionDirectionInBodyRadians: 0.0),
|
||||
"滚动运动系准备命令应被接受");
|
||||
AssertState(
|
||||
agent,
|
||||
FleetMemberAgentState.Preparing,
|
||||
"开始准备");
|
||||
AssertTrue(
|
||||
agent.UpdatePreparation(0.01) ==
|
||||
FleetMemberAgentState.Ready,
|
||||
"内存底盘舵轮应立即准备完成");
|
||||
|
||||
return agent;
|
||||
}
|
||||
|
||||
private static SteerWheel CreateWheel(
|
||||
float xMillimeters,
|
||||
float yMillimeters)
|
||||
{
|
||||
var speed = 0f;
|
||||
var angle = 0f;
|
||||
|
||||
return new SteerWheel(
|
||||
new Vector2(
|
||||
xMillimeters,
|
||||
yMillimeters),
|
||||
angleLowerLimit: -120f,
|
||||
angleUpperLimit: 120f,
|
||||
speedWriter: value => speed = value,
|
||||
speedReader: () => speed,
|
||||
angleWriter: value => angle = value,
|
||||
angleReader: () => angle);
|
||||
}
|
||||
|
||||
private static void AssertState(
|
||||
FleetMemberAgent agent,
|
||||
FleetMemberAgentState expected,
|
||||
string scenario)
|
||||
{
|
||||
AssertTrue(
|
||||
agent.State == expected,
|
||||
$"{scenario}后的状态应为{expected}，" +
|
||||
$"实际为{agent.State}。");
|
||||
}
|
||||
|
||||
private static void AssertNear(
|
||||
double? actual,
|
||||
double expected,
|
||||
string name)
|
||||
{
|
||||
AssertTrue(
|
||||
actual.HasValue &&
|
||||
Math.Abs(actual.Value - expected) <= 1e-9,
|
||||
$"{name}不正确，期望{expected:F6}，" +
|
||||
$"实际{actual?.ToString("F6") ?? "null"}。");
|
||||
}
|
||||
|
||||
private static void AssertTrue(
|
||||
bool condition,
|
||||
string message)
|
||||
{
|
||||
if (!condition)
|
||||
{
|
||||
throw new InvalidOperationException(message);
|
||||
}
|
||||
}
|
||||
|
||||
private static void AssertFalse(
|
||||
bool condition,
|
||||
string message)
|
||||
{
|
||||
AssertTrue(!condition, message);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,430 @@
|
||||
using System;
|
||||
using System.Numerics;
|
||||
using CommonUsage.Chassis;
|
||||
using MultiWheelC.Control.Abstractions;
|
||||
using MultiWheelC.Control.Allocation;
|
||||
using MultiWheelC.Fleet;
|
||||
using MultiWheelC.StateEstimation;
|
||||
using MultiWheelC.Trajectory;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Tests
|
||||
{
|
||||
internal static class FleetRuntimeTests
|
||||
{
|
||||
private const long PlanId = 21;
|
||||
|
||||
public static void Run()
|
||||
{
|
||||
VerifyTwoVehiclePlanBecomesActive();
|
||||
VerifyStaleCommandIsIgnored();
|
||||
VerifyInvalidCommandLatchesMemberFault();
|
||||
VerifyMissingReportFaultsLeader();
|
||||
VerifyMemberWatchdogStopsLocally();
|
||||
VerifyUnavailableMemberStateFaultsFleet();
|
||||
|
||||
Console.WriteLine(
|
||||
"FleetRuntime端到端测试通过，共6个场景。");
|
||||
}
|
||||
|
||||
private static void VerifyTwoVehiclePlanBecomesActive()
|
||||
{
|
||||
var fleet = CreateFleet();
|
||||
StartAndActivate(fleet);
|
||||
|
||||
AssertState(
|
||||
fleet.LeaderRuntime,
|
||||
FleetRuntimeState.Active,
|
||||
"主车正常激活");
|
||||
AssertState(
|
||||
fleet.MemberRuntime,
|
||||
FleetRuntimeState.Active,
|
||||
"从车正常激活");
|
||||
AssertTrue(
|
||||
fleet.LeaderRuntime.LastCoordinationOutput != null,
|
||||
"主车激活后应产生首周期协调输出");
|
||||
AssertTrue(
|
||||
fleet.MemberRuntime.LastAppliedCommandSequence > 0,
|
||||
"从车应确认已经执行主车命令");
|
||||
}
|
||||
|
||||
private static void VerifyStaleCommandIsIgnored()
|
||||
{
|
||||
var fleet = CreateFleet();
|
||||
StartAndActivate(fleet);
|
||||
var appliedSequence =
|
||||
fleet.MemberRuntime.LastAppliedCommandSequence;
|
||||
|
||||
fleet.LeaderTransport.SendCommand(
|
||||
new FleetCommand(
|
||||
PlanId,
|
||||
appliedSequence,
|
||||
targetVehicleId: 2,
|
||||
FleetCommandKind.Motion,
|
||||
motionDirectionInBodyRadians: 0.0,
|
||||
twistInVehicleBody:
|
||||
new Twist2D(1.0, 0.0, 0.0),
|
||||
validForSeconds: 0.2));
|
||||
fleet.MemberProvider.SetTimestamp(0.04);
|
||||
fleet.MemberRuntime.Update(0.04, 0.02);
|
||||
|
||||
AssertState(
|
||||
fleet.MemberRuntime,
|
||||
FleetRuntimeState.Active,
|
||||
"忽略旧序列命令");
|
||||
AssertTrue(
|
||||
fleet.MemberRuntime.LastAppliedCommandSequence ==
|
||||
appliedSequence,
|
||||
"旧序列命令不应更新最近执行序号");
|
||||
}
|
||||
|
||||
private static void VerifyMissingReportFaultsLeader()
|
||||
{
|
||||
var fleet = CreateFleet();
|
||||
AssertTrue(
|
||||
fleet.LeaderRuntime.StartRollingPlan(
|
||||
PlanId,
|
||||
fleet.Layout,
|
||||
CreateTrajectory()),
|
||||
"主车应成功启动测试任务");
|
||||
|
||||
fleet.LeaderRuntime.Update(0.0, 0.02);
|
||||
fleet.LeaderProvider.SetTimestamp(0.25);
|
||||
fleet.LeaderRuntime.Update(0.25, 0.02);
|
||||
|
||||
AssertState(
|
||||
fleet.LeaderRuntime,
|
||||
FleetRuntimeState.Faulted,
|
||||
"成员报告超时");
|
||||
AssertTrue(
|
||||
fleet.LeaderRuntime.LastFailureReason.Contains(
|
||||
"未收到成员车2"),
|
||||
"通信超时应指出缺失的成员车");
|
||||
AssertTrue(
|
||||
fleet.LeaderAgent.State ==
|
||||
FleetMemberAgentState.Idle,
|
||||
"主车故障后必须立即停止本车执行器");
|
||||
}
|
||||
|
||||
private static void VerifyInvalidCommandLatchesMemberFault()
|
||||
{
|
||||
var fleet = CreateFleet();
|
||||
StartAndActivate(fleet);
|
||||
|
||||
fleet.LeaderTransport.SendCommand(
|
||||
new FleetCommand(
|
||||
PlanId,
|
||||
fleet.MemberRuntime.LastAppliedCommandSequence + 1,
|
||||
targetVehicleId: 2,
|
||||
FleetCommandKind.Motion,
|
||||
motionDirectionInBodyRadians: 0.0,
|
||||
twistInVehicleBody: Twist2D.Zero,
|
||||
validForSeconds: double.NaN));
|
||||
fleet.MemberRuntime.Update(0.04, 0.02);
|
||||
|
||||
AssertState(
|
||||
fleet.MemberRuntime,
|
||||
FleetRuntimeState.Faulted,
|
||||
"非法命令字段触发并锁存本地故障");
|
||||
}
|
||||
|
||||
private static void VerifyMemberWatchdogStopsLocally()
|
||||
{
|
||||
var fleet = CreateFleet();
|
||||
StartAndActivate(fleet);
|
||||
|
||||
fleet.MemberProvider.SetTimestamp(0.25);
|
||||
fleet.MemberRuntime.Update(0.25, 0.02);
|
||||
|
||||
AssertState(
|
||||
fleet.MemberRuntime,
|
||||
FleetRuntimeState.Faulted,
|
||||
"从车命令看门狗超时");
|
||||
AssertTrue(
|
||||
fleet.MemberRuntime.LastFailureReason.Contains(
|
||||
"超时"),
|
||||
"从车看门狗应保留超时原因");
|
||||
}
|
||||
|
||||
private static void VerifyUnavailableMemberStateFaultsFleet()
|
||||
{
|
||||
var fleet = CreateFleet();
|
||||
StartAndActivate(fleet);
|
||||
|
||||
fleet.MemberProvider.IsAvailable = false;
|
||||
fleet.MemberRuntime.Update(0.04, 0.02);
|
||||
fleet.LeaderProvider.SetTimestamp(0.04);
|
||||
fleet.LeaderRuntime.Update(0.04, 0.02);
|
||||
|
||||
AssertState(
|
||||
fleet.MemberRuntime,
|
||||
FleetRuntimeState.Faulted,
|
||||
"从车状态不可用时本地停车");
|
||||
AssertState(
|
||||
fleet.LeaderRuntime,
|
||||
FleetRuntimeState.Faulted,
|
||||
"从车状态不可用时整队停车");
|
||||
}
|
||||
|
||||
private static void StartAndActivate(TestFleet fleet)
|
||||
{
|
||||
AssertTrue(
|
||||
fleet.LeaderRuntime.StartRollingPlan(
|
||||
PlanId,
|
||||
fleet.Layout,
|
||||
CreateTrajectory()),
|
||||
"主车应成功启动测试任务");
|
||||
|
||||
fleet.MemberRuntime.Update(0.0, 0.02);
|
||||
fleet.LeaderRuntime.Update(0.0, 0.02);
|
||||
fleet.MemberProvider.SetTimestamp(0.02);
|
||||
fleet.MemberRuntime.Update(0.02, 0.02);
|
||||
}
|
||||
|
||||
private static TestFleet CreateFleet()
|
||||
{
|
||||
var layout = new FleetLayout(
|
||||
new[]
|
||||
{
|
||||
new VehicleLayout(
|
||||
1,
|
||||
new Pose2D(-0.5, 0.0, 0.0)),
|
||||
new VehicleLayout(
|
||||
2,
|
||||
new Pose2D(0.5, 0.0, 0.0))
|
||||
});
|
||||
var network = new InMemoryFleetTransportNetwork(
|
||||
new[] { 1, 2 },
|
||||
leaderVehicleId: 1);
|
||||
var leaderTransport = network.CreateEndpoint(1);
|
||||
var memberTransport = network.CreateEndpoint(2);
|
||||
var leaderProvider = new MutableStateProvider(
|
||||
new Pose2D(-0.5, 0.0, 0.0));
|
||||
var memberProvider = new MutableStateProvider(
|
||||
new Pose2D(0.5, 0.0, 0.0));
|
||||
var leaderAgent = CreateAgent(1);
|
||||
var memberAgent = CreateAgent(2);
|
||||
|
||||
var leaderRuntime = new FleetRuntime(
|
||||
selfVehicleId: 1,
|
||||
leaderVehicleId: 1,
|
||||
leaderTransport,
|
||||
leaderAgent,
|
||||
leaderProvider,
|
||||
new FleetPreparationCoordinator(),
|
||||
CreateCoordinator(),
|
||||
new FleetSafetySupervisor(
|
||||
communicationTimeoutSeconds: 0.2),
|
||||
commandValidForSeconds: 0.2,
|
||||
preparationTimeoutSeconds: 1.0);
|
||||
var memberRuntime = new FleetRuntime(
|
||||
selfVehicleId: 2,
|
||||
leaderVehicleId: 1,
|
||||
memberTransport,
|
||||
memberAgent,
|
||||
memberProvider,
|
||||
commandValidForSeconds: 0.2);
|
||||
|
||||
return new TestFleet(
|
||||
layout,
|
||||
leaderTransport,
|
||||
leaderProvider,
|
||||
memberProvider,
|
||||
leaderAgent,
|
||||
leaderRuntime,
|
||||
memberRuntime);
|
||||
}
|
||||
|
||||
private static FleetCoordinator CreateCoordinator()
|
||||
{
|
||||
return new FleetCoordinator(
|
||||
new FleetStateEstimator(
|
||||
maximumMemberStateAgeSeconds: 0.5,
|
||||
maximumPositionDisagreementMeters: 0.2,
|
||||
maximumYawDisagreementRadians:
|
||||
AngleMath.DegreesToRadians(5.0)),
|
||||
new FleetController(
|
||||
new StraightLateralController(),
|
||||
new ZeroLongitudinalController(),
|
||||
new GcpCommandAllocator(Math.PI / 4.0),
|
||||
virtualControlPointRadiusMeters: 0.5),
|
||||
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)),
|
||||
memberPositionErrorWarningMeters: 0.02,
|
||||
maximumMemberPositionErrorMeters: 0.05,
|
||||
memberYawErrorWarningRadians:
|
||||
AngleMath.DegreesToRadians(1.0),
|
||||
maximumMemberYawErrorRadians:
|
||||
AngleMath.DegreesToRadians(3.0));
|
||||
}
|
||||
|
||||
private static Trajectory2D CreateTrajectory()
|
||||
{
|
||||
return new Trajectory2D(
|
||||
new[]
|
||||
{
|
||||
new TrajectoryPoint(
|
||||
0.0,
|
||||
Pose2D.Identity,
|
||||
0.0,
|
||||
0.2),
|
||||
new TrajectoryPoint(
|
||||
1.0,
|
||||
new Pose2D(1.0, 0.0, 0.0),
|
||||
0.0,
|
||||
0.2)
|
||||
});
|
||||
}
|
||||
|
||||
private static FleetMemberAgent CreateAgent(int vehicleId)
|
||||
{
|
||||
var chassis = new MultiWheelChassis();
|
||||
chassis.AddWheel(CreateWheel(-500f, 300f));
|
||||
chassis.AddWheel(CreateWheel(-500f, -300f));
|
||||
chassis.AddWheel(CreateWheel(500f, 300f));
|
||||
chassis.AddWheel(CreateWheel(500f, -300f));
|
||||
chassis.Initialize();
|
||||
|
||||
return new FleetMemberAgent(
|
||||
new MultiWheelChassisAdapter(chassis, vehicleId),
|
||||
alignmentToleranceRadians:
|
||||
AngleMath.DegreesToRadians(1.0),
|
||||
alignmentStableSeconds: 0.0);
|
||||
}
|
||||
|
||||
private static SteerWheel CreateWheel(float x, float y)
|
||||
{
|
||||
var speed = 0f;
|
||||
var angle = 0f;
|
||||
return new SteerWheel(
|
||||
new Vector2(x, y),
|
||||
angleLowerLimit: -120f,
|
||||
angleUpperLimit: 120f,
|
||||
speedWriter: value => speed = value,
|
||||
speedReader: () => speed,
|
||||
angleWriter: value => angle = value,
|
||||
angleReader: () => angle);
|
||||
}
|
||||
|
||||
private static void AssertState(
|
||||
FleetRuntime runtime,
|
||||
FleetRuntimeState expected,
|
||||
string scenario)
|
||||
{
|
||||
AssertTrue(
|
||||
runtime.State == expected,
|
||||
$"{scenario}状态错误：" +
|
||||
$"actual={runtime.State}, expected={expected}，" +
|
||||
$"reason={runtime.LastFailureReason}");
|
||||
}
|
||||
|
||||
private static void AssertTrue(bool condition, string message)
|
||||
{
|
||||
if (!condition)
|
||||
{
|
||||
throw new InvalidOperationException(message);
|
||||
}
|
||||
}
|
||||
|
||||
private sealed class MutableStateProvider :
|
||||
IVehicleStateProvider
|
||||
{
|
||||
private readonly Pose2D _poseInWorld;
|
||||
private double _timestampSeconds;
|
||||
|
||||
public MutableStateProvider(Pose2D poseInWorld)
|
||||
{
|
||||
_poseInWorld = poseInWorld;
|
||||
IsAvailable = true;
|
||||
}
|
||||
|
||||
public bool IsAvailable { get; set; }
|
||||
|
||||
public void SetTimestamp(double timestampSeconds)
|
||||
{
|
||||
_timestampSeconds = timestampSeconds;
|
||||
}
|
||||
|
||||
public bool TryGetState(out VehicleState state)
|
||||
{
|
||||
state = new VehicleState(
|
||||
_timestampSeconds,
|
||||
_poseInWorld,
|
||||
Twist2D.Zero,
|
||||
hasValidVelocityEstimate: true);
|
||||
return IsAvailable;
|
||||
}
|
||||
}
|
||||
|
||||
private sealed class StraightLateralController :
|
||||
ILateralController
|
||||
{
|
||||
public LateralControlCommand Compute(
|
||||
PathTrackingContext context)
|
||||
{
|
||||
return LateralControlCommand.Straight;
|
||||
}
|
||||
|
||||
public void Reset()
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
private sealed class ZeroLongitudinalController :
|
||||
ILongitudinalController
|
||||
{
|
||||
public double ComputeSpeedMetersPerSecond(
|
||||
PathTrackingContext context)
|
||||
{
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
public void Reset()
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
private sealed class TestFleet
|
||||
{
|
||||
public TestFleet(
|
||||
FleetLayout layout,
|
||||
InMemoryFleetTransport leaderTransport,
|
||||
MutableStateProvider leaderProvider,
|
||||
MutableStateProvider memberProvider,
|
||||
FleetMemberAgent leaderAgent,
|
||||
FleetRuntime leaderRuntime,
|
||||
FleetRuntime memberRuntime)
|
||||
{
|
||||
Layout = layout;
|
||||
LeaderTransport = leaderTransport;
|
||||
LeaderProvider = leaderProvider;
|
||||
MemberProvider = memberProvider;
|
||||
LeaderAgent = leaderAgent;
|
||||
LeaderRuntime = leaderRuntime;
|
||||
MemberRuntime = memberRuntime;
|
||||
}
|
||||
|
||||
public FleetLayout Layout { get; }
|
||||
|
||||
public InMemoryFleetTransport LeaderTransport { get; }
|
||||
|
||||
public MutableStateProvider LeaderProvider { get; }
|
||||
|
||||
public MutableStateProvider MemberProvider { get; }
|
||||
|
||||
public FleetMemberAgent LeaderAgent { get; }
|
||||
|
||||
public FleetRuntime LeaderRuntime { get; }
|
||||
|
||||
public FleetRuntime MemberRuntime { get; }
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,297 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.Fleet;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Tests
|
||||
{
|
||||
internal static class FleetSafetySupervisorTests
|
||||
{
|
||||
private const long PlanId = 7;
|
||||
private const double CurrentTimeSeconds = 10.0;
|
||||
private const double CommunicationTimeoutSeconds = 0.5;
|
||||
|
||||
public static void Run()
|
||||
{
|
||||
VerifyHealthyFleetContinues();
|
||||
VerifyMissingMemberStopsFleet();
|
||||
VerifyCommunicationTimeoutStopsFleet();
|
||||
VerifyUnavailableStateStopsFleet();
|
||||
VerifyMemberFaultStopsFleet();
|
||||
VerifyFailureCodeStopsFleet();
|
||||
VerifyPlanMismatchStopsFleet();
|
||||
VerifyStopIsLatchedUntilNewPlanStarts();
|
||||
|
||||
Console.WriteLine(
|
||||
"FleetSafetySupervisor测试通过，共8个场景。");
|
||||
}
|
||||
|
||||
private static void VerifyHealthyFleetContinues()
|
||||
{
|
||||
var supervisor = CreateStartedSupervisor();
|
||||
|
||||
var decision = supervisor.Evaluate(
|
||||
CreateLayout(),
|
||||
CreateHealthyStatuses(),
|
||||
CurrentTimeSeconds);
|
||||
|
||||
AssertFalse(
|
||||
decision.ShouldStop,
|
||||
"成员状态健康时不应停车");
|
||||
}
|
||||
|
||||
private static void VerifyMissingMemberStopsFleet()
|
||||
{
|
||||
var supervisor = CreateStartedSupervisor();
|
||||
|
||||
var decision = supervisor.Evaluate(
|
||||
CreateLayout(),
|
||||
new[] { CreateHealthyStatus(1) },
|
||||
CurrentTimeSeconds);
|
||||
|
||||
AssertStopFromVehicle(
|
||||
decision,
|
||||
2,
|
||||
"缺少成员报告");
|
||||
}
|
||||
|
||||
private static void VerifyCommunicationTimeoutStopsFleet()
|
||||
{
|
||||
var supervisor = CreateStartedSupervisor();
|
||||
var statuses = new[]
|
||||
{
|
||||
CreateHealthyStatus(1),
|
||||
new FleetMemberSafetyStatus(
|
||||
vehicleId: 2,
|
||||
planId: PlanId,
|
||||
isStateAvailable: true,
|
||||
isFaulted: false,
|
||||
failureCode: 0,
|
||||
lastAcceptedReportTimeSeconds: 9.4)
|
||||
};
|
||||
|
||||
var decision = supervisor.Evaluate(
|
||||
CreateLayout(),
|
||||
statuses,
|
||||
CurrentTimeSeconds);
|
||||
|
||||
AssertStopFromVehicle(
|
||||
decision,
|
||||
2,
|
||||
"通信超时");
|
||||
}
|
||||
|
||||
private static void VerifyUnavailableStateStopsFleet()
|
||||
{
|
||||
var supervisor = CreateStartedSupervisor();
|
||||
var statuses = new[]
|
||||
{
|
||||
CreateHealthyStatus(1),
|
||||
new FleetMemberSafetyStatus(
|
||||
vehicleId: 2,
|
||||
planId: PlanId,
|
||||
isStateAvailable: false,
|
||||
isFaulted: false,
|
||||
failureCode: 0,
|
||||
lastAcceptedReportTimeSeconds: 9.9)
|
||||
};
|
||||
|
||||
var decision = supervisor.Evaluate(
|
||||
CreateLayout(),
|
||||
statuses,
|
||||
CurrentTimeSeconds);
|
||||
|
||||
AssertStopFromVehicle(
|
||||
decision,
|
||||
2,
|
||||
"状态不可用");
|
||||
}
|
||||
|
||||
private static void VerifyMemberFaultStopsFleet()
|
||||
{
|
||||
var supervisor = CreateStartedSupervisor();
|
||||
var statuses = new[]
|
||||
{
|
||||
CreateHealthyStatus(1),
|
||||
new FleetMemberSafetyStatus(
|
||||
vehicleId: 2,
|
||||
planId: PlanId,
|
||||
isStateAvailable: true,
|
||||
isFaulted: true,
|
||||
failureCode: 0,
|
||||
lastAcceptedReportTimeSeconds: 9.9)
|
||||
};
|
||||
|
||||
var decision = supervisor.Evaluate(
|
||||
CreateLayout(),
|
||||
statuses,
|
||||
CurrentTimeSeconds);
|
||||
|
||||
AssertStopFromVehicle(
|
||||
decision,
|
||||
2,
|
||||
"成员故障状态");
|
||||
}
|
||||
|
||||
private static void VerifyFailureCodeStopsFleet()
|
||||
{
|
||||
var supervisor = CreateStartedSupervisor();
|
||||
var statuses = new[]
|
||||
{
|
||||
CreateHealthyStatus(1),
|
||||
new FleetMemberSafetyStatus(
|
||||
vehicleId: 2,
|
||||
planId: PlanId,
|
||||
isStateAvailable: true,
|
||||
isFaulted: false,
|
||||
failureCode: 42,
|
||||
lastAcceptedReportTimeSeconds: 9.9)
|
||||
};
|
||||
|
||||
var decision = supervisor.Evaluate(
|
||||
CreateLayout(),
|
||||
statuses,
|
||||
CurrentTimeSeconds);
|
||||
|
||||
AssertStopFromVehicle(
|
||||
decision,
|
||||
2,
|
||||
"成员故障码");
|
||||
}
|
||||
|
||||
private static void VerifyPlanMismatchStopsFleet()
|
||||
{
|
||||
var supervisor = CreateStartedSupervisor();
|
||||
var statuses = new[]
|
||||
{
|
||||
CreateHealthyStatus(1),
|
||||
new FleetMemberSafetyStatus(
|
||||
vehicleId: 2,
|
||||
planId: PlanId - 1,
|
||||
isStateAvailable: true,
|
||||
isFaulted: false,
|
||||
failureCode: 0,
|
||||
lastAcceptedReportTimeSeconds: 9.9)
|
||||
};
|
||||
|
||||
var decision = supervisor.Evaluate(
|
||||
CreateLayout(),
|
||||
statuses,
|
||||
CurrentTimeSeconds);
|
||||
|
||||
AssertStopFromVehicle(
|
||||
decision,
|
||||
2,
|
||||
"任务编号不一致");
|
||||
}
|
||||
|
||||
private static void VerifyStopIsLatchedUntilNewPlanStarts()
|
||||
{
|
||||
var supervisor = CreateStartedSupervisor();
|
||||
supervisor.Evaluate(
|
||||
CreateLayout(),
|
||||
new[] { CreateHealthyStatus(1) },
|
||||
CurrentTimeSeconds);
|
||||
|
||||
var latchedDecision = supervisor.Evaluate(
|
||||
CreateLayout(),
|
||||
CreateHealthyStatuses(),
|
||||
CurrentTimeSeconds);
|
||||
AssertTrue(
|
||||
latchedDecision.ShouldStop,
|
||||
"故障恢复后停车决定仍应锁存");
|
||||
|
||||
supervisor.Start(PlanId + 1);
|
||||
var recoveredDecision = supervisor.Evaluate(
|
||||
CreateLayout(),
|
||||
new[]
|
||||
{
|
||||
CreateHealthyStatus(1, PlanId + 1),
|
||||
CreateHealthyStatus(2, PlanId + 1)
|
||||
},
|
||||
CurrentTimeSeconds);
|
||||
AssertFalse(
|
||||
recoveredDecision.ShouldStop,
|
||||
"开始新任务后应清除旧任务停车锁存");
|
||||
}
|
||||
|
||||
private static FleetSafetySupervisor
|
||||
CreateStartedSupervisor()
|
||||
{
|
||||
var supervisor = new FleetSafetySupervisor(
|
||||
CommunicationTimeoutSeconds);
|
||||
supervisor.Start(PlanId);
|
||||
return supervisor;
|
||||
}
|
||||
|
||||
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<FleetMemberSafetyStatus>
|
||||
CreateHealthyStatuses()
|
||||
{
|
||||
return new[]
|
||||
{
|
||||
CreateHealthyStatus(1),
|
||||
CreateHealthyStatus(2)
|
||||
};
|
||||
}
|
||||
|
||||
private static FleetMemberSafetyStatus CreateHealthyStatus(
|
||||
int vehicleId,
|
||||
long planId = PlanId)
|
||||
{
|
||||
return new FleetMemberSafetyStatus(
|
||||
vehicleId,
|
||||
planId,
|
||||
isStateAvailable: true,
|
||||
isFaulted: false,
|
||||
failureCode: 0,
|
||||
lastAcceptedReportTimeSeconds: 9.9);
|
||||
}
|
||||
|
||||
private static void AssertStopFromVehicle(
|
||||
FleetSafetyDecision decision,
|
||||
int expectedVehicleId,
|
||||
string scenario)
|
||||
{
|
||||
AssertTrue(
|
||||
decision.ShouldStop,
|
||||
$"{scenario}时应停车");
|
||||
AssertTrue(
|
||||
decision.SourceVehicleId == expectedVehicleId,
|
||||
$"{scenario}的来源车辆不正确");
|
||||
AssertTrue(
|
||||
!string.IsNullOrWhiteSpace(decision.Reason),
|
||||
$"{scenario}应提供停车原因");
|
||||
}
|
||||
|
||||
private static void AssertTrue(
|
||||
bool condition,
|
||||
string message)
|
||||
{
|
||||
if (!condition)
|
||||
{
|
||||
throw new InvalidOperationException(message);
|
||||
}
|
||||
}
|
||||
|
||||
private static void AssertFalse(
|
||||
bool condition,
|
||||
string message)
|
||||
{
|
||||
AssertTrue(!condition, message);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,240 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using MultiWheelC.Fleet;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Tests
|
||||
{
|
||||
// 为同一测试进程中的各车辆端点提供共享FIFO消息队列。
|
||||
internal sealed class InMemoryFleetTransportNetwork
|
||||
{
|
||||
private readonly SharedState _sharedState;
|
||||
private readonly HashSet<int> _createdEndpointIds =
|
||||
new HashSet<int>();
|
||||
|
||||
public InMemoryFleetTransportNetwork(
|
||||
IReadOnlyList<int> vehicleIds,
|
||||
int leaderVehicleId)
|
||||
{
|
||||
_sharedState = new SharedState(
|
||||
vehicleIds,
|
||||
leaderVehicleId);
|
||||
}
|
||||
|
||||
public InMemoryFleetTransport CreateEndpoint(
|
||||
int vehicleId)
|
||||
{
|
||||
lock (_sharedState.SyncRoot)
|
||||
{
|
||||
if (!_sharedState.CommandQueues.ContainsKey(
|
||||
vehicleId))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(vehicleId),
|
||||
$"车辆{vehicleId}不属于当前内存车队网络。");
|
||||
}
|
||||
|
||||
if (!_createdEndpointIds.Add(vehicleId))
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"车辆{vehicleId}的内存通信端点已经创建。");
|
||||
}
|
||||
}
|
||||
|
||||
return new InMemoryFleetTransport(
|
||||
_sharedState,
|
||||
vehicleId);
|
||||
}
|
||||
|
||||
internal sealed class SharedState
|
||||
{
|
||||
public SharedState(
|
||||
IReadOnlyList<int> vehicleIds,
|
||||
int leaderVehicleId)
|
||||
{
|
||||
if (vehicleIds == null)
|
||||
{
|
||||
throw new ArgumentNullException(
|
||||
nameof(vehicleIds));
|
||||
}
|
||||
|
||||
if (vehicleIds.Count == 0)
|
||||
{
|
||||
throw new ArgumentException(
|
||||
"内存车队网络至少需要一辆车。",
|
||||
nameof(vehicleIds));
|
||||
}
|
||||
|
||||
if (leaderVehicleId <= 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(leaderVehicleId),
|
||||
"主车编号必须大于零。");
|
||||
}
|
||||
|
||||
CommandQueues =
|
||||
new Dictionary<int, Queue<FleetCommand>>();
|
||||
|
||||
for (var index = 0;
|
||||
index < vehicleIds.Count;
|
||||
index++)
|
||||
{
|
||||
var vehicleId = vehicleIds[index];
|
||||
if (vehicleId <= 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(vehicleIds),
|
||||
$"第{index}辆车的编号必须大于零。");
|
||||
}
|
||||
|
||||
if (CommandQueues.ContainsKey(vehicleId))
|
||||
{
|
||||
throw new ArgumentException(
|
||||
$"内存车队网络包含重复车号{vehicleId}。",
|
||||
nameof(vehicleIds));
|
||||
}
|
||||
|
||||
CommandQueues.Add(
|
||||
vehicleId,
|
||||
new Queue<FleetCommand>());
|
||||
}
|
||||
|
||||
if (!CommandQueues.ContainsKey(
|
||||
leaderVehicleId))
|
||||
{
|
||||
throw new ArgumentException(
|
||||
$"主车{leaderVehicleId}不在车辆列表中。",
|
||||
nameof(leaderVehicleId));
|
||||
}
|
||||
|
||||
LeaderVehicleId = leaderVehicleId;
|
||||
}
|
||||
|
||||
public object SyncRoot { get; } = new object();
|
||||
|
||||
public int LeaderVehicleId { get; }
|
||||
|
||||
public Dictionary<int, Queue<FleetCommand>>
|
||||
CommandQueues { get; }
|
||||
|
||||
public Queue<FleetMemberReport> ReportQueue { get; } =
|
||||
new Queue<FleetMemberReport>();
|
||||
}
|
||||
}
|
||||
|
||||
// 单辆模拟车辆持有的通信端点；只负责消息路由，不解释控制语义。
|
||||
internal sealed class InMemoryFleetTransport : IFleetTransport
|
||||
{
|
||||
private readonly InMemoryFleetTransportNetwork.SharedState
|
||||
_sharedState;
|
||||
private readonly int _localVehicleId;
|
||||
|
||||
internal InMemoryFleetTransport(
|
||||
InMemoryFleetTransportNetwork.SharedState sharedState,
|
||||
int localVehicleId)
|
||||
{
|
||||
_sharedState = sharedState ??
|
||||
throw new ArgumentNullException(
|
||||
nameof(sharedState));
|
||||
_localVehicleId = localVehicleId;
|
||||
}
|
||||
|
||||
public void SendCommand(FleetCommand command)
|
||||
{
|
||||
if (_localVehicleId !=
|
||||
_sharedState.LeaderVehicleId)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"只有主车通信端点可以发送车队命令。");
|
||||
}
|
||||
|
||||
lock (_sharedState.SyncRoot)
|
||||
{
|
||||
if (command.TargetVehicleId ==
|
||||
FleetProtocol.BroadcastVehicleId)
|
||||
{
|
||||
foreach (var pair in
|
||||
_sharedState.CommandQueues)
|
||||
{
|
||||
// 主车本地命令由运行入口直接执行，不通过通信回环。
|
||||
if (pair.Key != _localVehicleId)
|
||||
{
|
||||
pair.Value.Enqueue(command);
|
||||
}
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
if (!_sharedState.CommandQueues.TryGetValue(
|
||||
command.TargetVehicleId,
|
||||
out var queue))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(command),
|
||||
$"目标车辆{command.TargetVehicleId}不存在。");
|
||||
}
|
||||
|
||||
queue.Enqueue(command);
|
||||
}
|
||||
}
|
||||
|
||||
public void SendReport(FleetMemberReport report)
|
||||
{
|
||||
if (report.VehicleId != _localVehicleId)
|
||||
{
|
||||
throw new ArgumentException(
|
||||
$"车辆{_localVehicleId}不能发送属于车辆" +
|
||||
$"{report.VehicleId}的状态报告。",
|
||||
nameof(report));
|
||||
}
|
||||
|
||||
lock (_sharedState.SyncRoot)
|
||||
{
|
||||
_sharedState.ReportQueue.Enqueue(report);
|
||||
}
|
||||
}
|
||||
|
||||
public bool TryReceiveCommand(
|
||||
out FleetCommand command)
|
||||
{
|
||||
lock (_sharedState.SyncRoot)
|
||||
{
|
||||
var queue =
|
||||
_sharedState.CommandQueues[_localVehicleId];
|
||||
if (queue.Count == 0)
|
||||
{
|
||||
command = default;
|
||||
return false;
|
||||
}
|
||||
|
||||
command = queue.Dequeue();
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
public bool TryReceiveReport(
|
||||
out FleetMemberReport report)
|
||||
{
|
||||
if (_localVehicleId !=
|
||||
_sharedState.LeaderVehicleId)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"只有主车通信端点可以接收成员状态报告。");
|
||||
}
|
||||
|
||||
lock (_sharedState.SyncRoot)
|
||||
{
|
||||
if (_sharedState.ReportQueue.Count == 0)
|
||||
{
|
||||
report = default;
|
||||
return false;
|
||||
}
|
||||
|
||||
report =
|
||||
_sharedState.ReportQueue.Dequeue();
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,198 @@
|
||||
using System;
|
||||
using MultiWheelC.Fleet;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Tests
|
||||
{
|
||||
internal static class InMemoryFleetTransportTests
|
||||
{
|
||||
public static void Run()
|
||||
{
|
||||
VerifyTargetedCommandRoutingAndFifoOrder();
|
||||
VerifyBroadcastReachesAllFollowersOnly();
|
||||
VerifyMemberReportReturnsToLeader();
|
||||
VerifyEndpointRolesAndVehicleIdentity();
|
||||
|
||||
Console.WriteLine(
|
||||
"InMemoryFleetTransport测试通过，共4个场景。");
|
||||
}
|
||||
|
||||
private static void VerifyTargetedCommandRoutingAndFifoOrder()
|
||||
{
|
||||
var network = CreateNetwork();
|
||||
var leader = network.CreateEndpoint(1);
|
||||
var member2 = network.CreateEndpoint(2);
|
||||
var member3 = network.CreateEndpoint(3);
|
||||
|
||||
leader.SendCommand(CreateCommand(2, sequenceNumber: 1));
|
||||
leader.SendCommand(CreateCommand(2, sequenceNumber: 2));
|
||||
|
||||
AssertTrue(
|
||||
member2.TryReceiveCommand(out var first) &&
|
||||
first.SequenceNumber == 1,
|
||||
"定向命令第一条应到达目标车辆");
|
||||
AssertTrue(
|
||||
member2.TryReceiveCommand(out var second) &&
|
||||
second.SequenceNumber == 2,
|
||||
"定向命令应保持FIFO顺序");
|
||||
AssertFalse(
|
||||
member2.TryReceiveCommand(out _),
|
||||
"目标车辆不应收到额外命令");
|
||||
AssertFalse(
|
||||
member3.TryReceiveCommand(out _),
|
||||
"其他成员不应收到定向命令");
|
||||
}
|
||||
|
||||
private static void VerifyBroadcastReachesAllFollowersOnly()
|
||||
{
|
||||
var network = CreateNetwork();
|
||||
var leader = network.CreateEndpoint(1);
|
||||
var member2 = network.CreateEndpoint(2);
|
||||
var member3 = network.CreateEndpoint(3);
|
||||
|
||||
leader.SendCommand(
|
||||
CreateCommand(
|
||||
FleetProtocol.BroadcastVehicleId,
|
||||
sequenceNumber: 3));
|
||||
|
||||
AssertTrue(
|
||||
member2.TryReceiveCommand(out var command2) &&
|
||||
command2.SequenceNumber == 3,
|
||||
"广播命令应到达成员车2");
|
||||
AssertTrue(
|
||||
member3.TryReceiveCommand(out var command3) &&
|
||||
command3.SequenceNumber == 3,
|
||||
"广播命令应到达成员车3");
|
||||
AssertFalse(
|
||||
leader.TryReceiveCommand(out _),
|
||||
"主车本地命令不应通过通信层回环");
|
||||
}
|
||||
|
||||
private static void VerifyMemberReportReturnsToLeader()
|
||||
{
|
||||
var network = CreateNetwork();
|
||||
var leader = network.CreateEndpoint(1);
|
||||
var member2 = network.CreateEndpoint(2);
|
||||
|
||||
member2.SendReport(
|
||||
CreateReport(
|
||||
vehicleId: 2,
|
||||
sequenceNumber: 8));
|
||||
|
||||
AssertTrue(
|
||||
leader.TryReceiveReport(out var report),
|
||||
"主车应收到成员报告");
|
||||
AssertTrue(
|
||||
report.VehicleId == 2 &&
|
||||
report.SequenceNumber == 8,
|
||||
"主车收到的成员报告内容不正确");
|
||||
AssertFalse(
|
||||
leader.TryReceiveReport(out _),
|
||||
"报告队列取空后应返回false");
|
||||
}
|
||||
|
||||
private static void VerifyEndpointRolesAndVehicleIdentity()
|
||||
{
|
||||
var network = CreateNetwork();
|
||||
var leader = network.CreateEndpoint(1);
|
||||
var member2 = network.CreateEndpoint(2);
|
||||
|
||||
AssertThrows<InvalidOperationException>(
|
||||
() => member2.SendCommand(
|
||||
CreateCommand(1, sequenceNumber: 1)),
|
||||
"从车不能发送车队命令");
|
||||
AssertThrows<InvalidOperationException>(
|
||||
() => member2.TryReceiveReport(out _),
|
||||
"从车不能消费全队成员报告");
|
||||
AssertThrows<ArgumentException>(
|
||||
() => member2.SendReport(
|
||||
CreateReport(
|
||||
vehicleId: 1,
|
||||
sequenceNumber: 1)),
|
||||
"端点不能冒用其他车辆身份");
|
||||
|
||||
leader.SendReport(
|
||||
CreateReport(
|
||||
vehicleId: 1,
|
||||
sequenceNumber: 2));
|
||||
AssertTrue(
|
||||
leader.TryReceiveReport(out var leaderReport) &&
|
||||
leaderReport.VehicleId == 1,
|
||||
"主车作为成员时也应能上报本车状态");
|
||||
}
|
||||
|
||||
private static InMemoryFleetTransportNetwork CreateNetwork()
|
||||
{
|
||||
return new InMemoryFleetTransportNetwork(
|
||||
new[] { 1, 2, 3 },
|
||||
leaderVehicleId: 1);
|
||||
}
|
||||
|
||||
private static FleetCommand CreateCommand(
|
||||
int targetVehicleId,
|
||||
long sequenceNumber)
|
||||
{
|
||||
return new FleetCommand(
|
||||
planId: 5,
|
||||
sequenceNumber,
|
||||
targetVehicleId,
|
||||
FleetCommandKind.Stop,
|
||||
motionDirectionInBodyRadians: 0.0,
|
||||
twistInVehicleBody: Twist2D.Zero,
|
||||
validForSeconds: 0.5);
|
||||
}
|
||||
|
||||
private static FleetMemberReport CreateReport(
|
||||
int vehicleId,
|
||||
long sequenceNumber)
|
||||
{
|
||||
return new FleetMemberReport(
|
||||
vehicleId,
|
||||
planId: 5,
|
||||
sequenceNumber,
|
||||
sampleTimestampSeconds: 1.0,
|
||||
poseInCommonWorld: Pose2D.Identity,
|
||||
twistAtVehicleOriginInCommonWorld:
|
||||
Twist2D.Zero,
|
||||
isStateAvailable: true,
|
||||
hasValidVelocityEstimate: true,
|
||||
FleetMemberState.Active,
|
||||
lastAppliedCommandSequence: 1);
|
||||
}
|
||||
|
||||
private static void AssertThrows<TException>(
|
||||
Action action,
|
||||
string scenario)
|
||||
where TException : Exception
|
||||
{
|
||||
try
|
||||
{
|
||||
action();
|
||||
}
|
||||
catch (TException)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
throw new InvalidOperationException(
|
||||
$"{scenario}时应抛出{typeof(TException).Name}。");
|
||||
}
|
||||
|
||||
private static void AssertTrue(
|
||||
bool condition,
|
||||
string message)
|
||||
{
|
||||
if (!condition)
|
||||
{
|
||||
throw new InvalidOperationException(message);
|
||||
}
|
||||
}
|
||||
|
||||
private static void AssertFalse(
|
||||
bool condition,
|
||||
string message)
|
||||
{
|
||||
AssertTrue(!condition, message);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -11,4 +11,10 @@
|
||||
<ProjectReference Include="..\MultiWheelC\MultiWheelC.csproj" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<Reference Include="CommonUsage">
|
||||
<HintPath>..\ref\CommonUsage.dll</HintPath>
|
||||
</Reference>
|
||||
</ItemGroup>
|
||||
|
||||
</Project>
|
||||
|
||||
@@ -42,6 +42,10 @@ namespace MultiWheelC.Tests
|
||||
FleetMemberCommandCorrectorTests.Run();
|
||||
FleetCoordinatorTests.Run();
|
||||
FleetPreparationCoordinatorTests.Run();
|
||||
FleetMemberAgentTests.Run();
|
||||
FleetSafetySupervisorTests.Run();
|
||||
InMemoryFleetTransportTests.Run();
|
||||
FleetRuntimeTests.Run();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
|
||||
Reference in New Issue
Block a user