ParkingRobot/MultiWheelC.Tests/FleetRuntimeTests.cs

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; }
        }
    }
}