431 lines
14 KiB
C#
431 lines
14 KiB
C#
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; }
|
||
}
|
||
}
|
||
}
|