贯通车队运行链并支持轨迹自动推导β

2026-08-25 17:59:18 +08:00
parent 95c0b19a26
commit 17092c2766
37 changed files with 3632 additions and 37 deletions
@@ -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; }
+        }
+    }
+}