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

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