ParkingRobot/MultiWheelC.Tests/FleetControllerTests.cs

using System;
using MultiWheelC.Control.Abstractions;
using MultiWheelC.Control.Allocation;
using MultiWheelC.Fleet;
using MultiWheelC.Trajectory;
using MyParking.Shared;

namespace MultiWheelC.Tests
{
    internal static class FleetControllerTests
    {
        private const double Tolerance = 1e-9;

        public static void Run()
        {
            VerifyInactiveControllerStops();
            VerifyStraightCommand();
            VerifyInvalidVelocityUsesReferenceSpeed();
            VerifyCompletionStops();
            VerifyExcessiveTrackingErrorFaults();
            VerifyCancelStops();

            Console.WriteLine(
                "FleetController车队中心控制测试通过。共6个场景。");
        }

        private static void VerifyInactiveControllerStops()
        {
            var controller = CreateController();
            var result = controller.ComputeCommand(
                CreateState(0.5, 0.0, 0.0, true),
                0.02,
                out var command);

            AssertResult(
                result,
                FleetControlCycleResult.Inactive,
                "未启动控制器");
            AssertStop(command, "未启动控制器");
        }

        private static void VerifyStraightCommand()
        {
            var controller = CreateController();
            controller.Start(CreateStraightTrajectory());

            var result = controller.ComputeCommand(
                CreateState(0.5, 0.0, 0.4, true),
                0.02,
                out var command);

            AssertResult(
                result,
                FleetControlCycleResult.CommandGenerated,
                "直线控制");
            AssertNear(
                command.ReferencePointInFleet.XMeters,
                0.0,
                "直线控制参考点X");
            AssertNear(
                command.ReferencePointInFleet.YMeters,
                0.0,
                "直线控制参考点Y");
            AssertTwist(
                command.TwistAtReferencePoint,
                0.4,
                0.0,
                0.0,
                "直线控制");
        }

        private static void VerifyInvalidVelocityUsesReferenceSpeed()
        {
            var controller = CreateController();
            controller.Start(CreateStraightTrajectory());

            var result = controller.ComputeCommand(
                CreateState(0.5, 0.0, 0.0, false),
                0.02,
                out var command);

            AssertResult(
                result,
                FleetControlCycleResult.CommandGenerated,
                "速度尚未初始化");
            AssertTwist(
                command.TwistAtReferencePoint,
                0.4,
                0.0,
                0.0,
                "速度尚未初始化");
        }

        private static void VerifyCompletionStops()
        {
            var controller = CreateController();
            controller.Start(CreateStraightTrajectory());

            var result = controller.ComputeCommand(
                CreateState(1.0, 0.0, 0.0, true),
                0.02,
                out var command);

            AssertResult(
                result,
                FleetControlCycleResult.Completed,
                "终点完成");
            AssertStop(command, "终点完成");

            if (controller.IsActive || !controller.IsCompleted)
            {
                throw new InvalidOperationException(
                    "终点完成后控制器状态错误。");
            }
        }

        private static void VerifyExcessiveTrackingErrorFaults()
        {
            var controller = CreateController();
            controller.Start(CreateStraightTrajectory());

            var result = controller.ComputeCommand(
                CreateState(0.5, 0.5, 0.0, true),
                0.02,
                out var command);

            AssertResult(
                result,
                FleetControlCycleResult.Faulted,
                "轨迹偏离保护");
            AssertStop(command, "轨迹偏离保护");

            if (string.IsNullOrWhiteSpace(
                    controller.LastFailureReason))
            {
                throw new InvalidOperationException(
                    "轨迹偏离故障没有保存原因。");
            }
        }

        private static void VerifyCancelStops()
        {
            var controller = CreateController();
            controller.Start(CreateStraightTrajectory());
            controller.Cancel();

            var result = controller.ComputeCommand(
                CreateState(0.5, 0.0, 0.0, true),
                0.02,
                out var command);

            AssertResult(
                result,
                FleetControlCycleResult.Inactive,
                "取消控制");
            AssertStop(command, "取消控制");
        }

        private static FleetController CreateController()
        {
            return new FleetController(
                new StraightLateralController(),
                new ReferenceLongitudinalController(),
                new GcpCommandAllocator(
                    Math.PI / 4.0),
                virtualControlPointRadiusMeters: 0.5);
        }

        private static Trajectory2D CreateStraightTrajectory()
        {
            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 FleetState CreateState(
            double xMeters,
            double yMeters,
            double vxMetersPerSecond,
            bool hasValidVelocityEstimate)
        {
            return new FleetState(
                sampleTimestampSeconds: 1.0,
                fleetPoseInWorld: new Pose2D(
                    xMeters,
                    yMeters,
                    0.0),
                twistAtFleetOriginInWorld: new Twist2D(
                    vxMetersPerSecond,
                    0.0,
                    0.0),
                hasValidVelocityEstimate:
                    hasValidVelocityEstimate);
        }

        private static void AssertResult(
            FleetControlCycleResult actual,
            FleetControlCycleResult expected,
            string scenario)
        {
            if (actual != expected)
            {
                throw new InvalidOperationException(
                    $"{scenario}结果错误：" +
                    $"actual={actual}, expected={expected}。");
            }
        }

        private static void AssertStop(
            FleetMotionCommand command,
            string scenario)
        {
            AssertTwist(
                command.TwistAtReferencePoint,
                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}。");
            }
        }

        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()
            {
            }
        }
    }
}