diff --git a/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll b/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll index 2c0496a..d284e2e 100644 Binary files a/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll and b/MedullaAdapter/build/Medulla/plugins/CommonUsage.dll differ diff --git a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll index 13f09f7..6a3cd4b 100644 Binary files a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll and b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll differ diff --git a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb index 1f8a518..dc4c9c6 100644 Binary files a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb and b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb differ diff --git a/MultiWheelC.Tests/FleetMemberAgentTests.cs b/MultiWheelC.Tests/FleetMemberAgentTests.cs new file mode 100644 index 0000000..65b15e4 --- /dev/null +++ b/MultiWheelC.Tests/FleetMemberAgentTests.cs @@ -0,0 +1,240 @@ +using System; +using System.Numerics; +using CommonUsage.Chassis; +using MultiWheelC.Fleet; +using MyParking.Shared; + +namespace MultiWheelC.Tests +{ + internal static class FleetMemberAgentTests + { + private const long PlanId = 11; + private const int VehicleId = 2; + + public static void Run() + { + VerifyActiveWatchdogExpires(); + VerifyValidMotionCommandRefreshesDeadline(); + VerifyRepeatedActivationDoesNotRefreshDeadline(); + VerifyLateActivationCannotResumeMotion(); + VerifyStopClearsWatchdog(); + + Console.WriteLine( + "FleetMemberAgent本地命令看门狗测试通过，共5个场景。"); + } + + private static void VerifyActiveWatchdogExpires() + { + var agent = CreateReadyAgent(); + AssertTrue( + agent.Activate( + PlanId, + commandReceivedTimeSeconds: 10.0, + validForSeconds: 0.5), + "成员车应当成功激活"); + + AssertTrue( + agent.UpdateCommandWatchdog(10.5), + "截止时刻仍应视为有效"); + AssertFalse( + agent.UpdateCommandWatchdog(10.501), + "超过命令有效期后应停车"); + AssertState( + agent, + FleetMemberAgentState.Faulted, + "命令超时"); + AssertTrue( + !string.IsNullOrWhiteSpace( + agent.LastFailureReason), + "命令超时应保留故障原因"); + } + + private static void VerifyValidMotionCommandRefreshesDeadline() + { + var agent = CreateReadyAgent(); + agent.Activate( + PlanId, + commandReceivedTimeSeconds: 20.0, + validForSeconds: 0.5); + + var accepted = agent.Execute( + PlanId, + new FleetMemberCommand( + VehicleId, + Twist2D.Zero), + commandReceivedTimeSeconds: 20.4, + validForSeconds: 0.7); + + AssertTrue( + accepted, + "有效速度命令应被接受"); + AssertNear( + agent.LastAcceptedCommandTimeSeconds, + 20.4, + "最近命令接收时间"); + AssertNear( + agent.CommandDeadlineSeconds, + 21.1, + "速度命令刷新后的截止时间"); + AssertTrue( + agent.UpdateCommandWatchdog(20.8), + "刷新截止时间后车辆应保持Active"); + } + + private static void VerifyRepeatedActivationDoesNotRefreshDeadline() + { + var agent = CreateReadyAgent(); + agent.Activate( + PlanId, + commandReceivedTimeSeconds: 30.0, + validForSeconds: 0.5); + + AssertTrue( + agent.Activate( + PlanId, + commandReceivedTimeSeconds: 30.4, + validForSeconds: 0.5), + "未超时的重复激活命令应允许幂等确认"); + AssertNear( + agent.CommandDeadlineSeconds, + 30.5, + "重复激活不能替代运动命令刷新截止时间"); + AssertFalse( + agent.UpdateCommandWatchdog(30.501), + "没有收到运动命令时仍应按最初激活期限停车"); + } + + private static void VerifyLateActivationCannotResumeMotion() + { + var agent = CreateReadyAgent(); + agent.Activate( + PlanId, + commandReceivedTimeSeconds: 40.0, + validForSeconds: 0.5); + + AssertFalse( + agent.Activate( + PlanId, + commandReceivedTimeSeconds: 40.6, + validForSeconds: 0.5), + "迟到的激活命令不能恢复已经失联的车辆"); + AssertState( + agent, + FleetMemberAgentState.Faulted, + "迟到激活命令"); + } + + private static void VerifyStopClearsWatchdog() + { + var agent = CreateReadyAgent(); + agent.Activate( + PlanId, + commandReceivedTimeSeconds: 50.0, + validForSeconds: 0.5); + + agent.Stop(); + + AssertState( + agent, + FleetMemberAgentState.Idle, + "正常停止"); + AssertTrue( + !agent.LastAcceptedCommandTimeSeconds.HasValue && + !agent.CommandDeadlineSeconds.HasValue, + "正常停止后应清除命令看门狗"); + } + + private static FleetMemberAgent CreateReadyAgent() + { + 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(); + + var agent = new FleetMemberAgent( + new MultiWheelChassisAdapter( + chassis, + VehicleId), + alignmentToleranceRadians: + AngleMath.DegreesToRadians(1.0), + alignmentStableSeconds: 0.0); + + AssertTrue( + agent.BeginRollingPreparation( + PlanId, + motionDirectionInBodyRadians: 0.0), + "滚动运动系准备命令应被接受"); + AssertState( + agent, + FleetMemberAgentState.Preparing, + "开始准备"); + AssertTrue( + agent.UpdatePreparation(0.01) == + FleetMemberAgentState.Ready, + "内存底盘舵轮应立即准备完成"); + + return agent; + } + + private static SteerWheel CreateWheel( + float xMillimeters, + float yMillimeters) + { + var speed = 0f; + var angle = 0f; + + return new SteerWheel( + new Vector2( + xMillimeters, + yMillimeters), + angleLowerLimit: -120f, + angleUpperLimit: 120f, + speedWriter: value => speed = value, + speedReader: () => speed, + angleWriter: value => angle = value, + angleReader: () => angle); + } + + private static void AssertState( + FleetMemberAgent agent, + FleetMemberAgentState expected, + string scenario) + { + AssertTrue( + agent.State == expected, + $"{scenario}后的状态应为{expected}，" + + $"实际为{agent.State}。"); + } + + private static void AssertNear( + double? actual, + double expected, + string name) + { + AssertTrue( + actual.HasValue && + Math.Abs(actual.Value - expected) <= 1e-9, + $"{name}不正确，期望{expected:F6}，" + + $"实际{actual?.ToString("F6") ?? "null"}。"); + } + + 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); + } + } +} diff --git a/MultiWheelC.Tests/FleetRuntimeTests.cs b/MultiWheelC.Tests/FleetRuntimeTests.cs new file mode 100644 index 0000000..b9fc40d --- /dev/null +++ b/MultiWheelC.Tests/FleetRuntimeTests.cs @@ -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; } + } + } +} diff --git a/MultiWheelC.Tests/FleetSafetySupervisorTests.cs b/MultiWheelC.Tests/FleetSafetySupervisorTests.cs new file mode 100644 index 0000000..3d60ea5 --- /dev/null +++ b/MultiWheelC.Tests/FleetSafetySupervisorTests.cs @@ -0,0 +1,297 @@ +using System; +using System.Collections.Generic; +using MultiWheelC.Fleet; +using MyParking.Shared; + +namespace MultiWheelC.Tests +{ + internal static class FleetSafetySupervisorTests + { + private const long PlanId = 7; + private const double CurrentTimeSeconds = 10.0; + private const double CommunicationTimeoutSeconds = 0.5; + + public static void Run() + { + VerifyHealthyFleetContinues(); + VerifyMissingMemberStopsFleet(); + VerifyCommunicationTimeoutStopsFleet(); + VerifyUnavailableStateStopsFleet(); + VerifyMemberFaultStopsFleet(); + VerifyFailureCodeStopsFleet(); + VerifyPlanMismatchStopsFleet(); + VerifyStopIsLatchedUntilNewPlanStarts(); + + Console.WriteLine( + "FleetSafetySupervisor测试通过，共8个场景。"); + } + + private static void VerifyHealthyFleetContinues() + { + var supervisor = CreateStartedSupervisor(); + + var decision = supervisor.Evaluate( + CreateLayout(), + CreateHealthyStatuses(), + CurrentTimeSeconds); + + AssertFalse( + decision.ShouldStop, + "成员状态健康时不应停车"); + } + + private static void VerifyMissingMemberStopsFleet() + { + var supervisor = CreateStartedSupervisor(); + + var decision = supervisor.Evaluate( + CreateLayout(), + new[] { CreateHealthyStatus(1) }, + CurrentTimeSeconds); + + AssertStopFromVehicle( + decision, + 2, + "缺少成员报告"); + } + + private static void VerifyCommunicationTimeoutStopsFleet() + { + var supervisor = CreateStartedSupervisor(); + var statuses = new[] + { + CreateHealthyStatus(1), + new FleetMemberSafetyStatus( + vehicleId: 2, + planId: PlanId, + isStateAvailable: true, + isFaulted: false, + failureCode: 0, + lastAcceptedReportTimeSeconds: 9.4) + }; + + var decision = supervisor.Evaluate( + CreateLayout(), + statuses, + CurrentTimeSeconds); + + AssertStopFromVehicle( + decision, + 2, + "通信超时"); + } + + private static void VerifyUnavailableStateStopsFleet() + { + var supervisor = CreateStartedSupervisor(); + var statuses = new[] + { + CreateHealthyStatus(1), + new FleetMemberSafetyStatus( + vehicleId: 2, + planId: PlanId, + isStateAvailable: false, + isFaulted: false, + failureCode: 0, + lastAcceptedReportTimeSeconds: 9.9) + }; + + var decision = supervisor.Evaluate( + CreateLayout(), + statuses, + CurrentTimeSeconds); + + AssertStopFromVehicle( + decision, + 2, + "状态不可用"); + } + + private static void VerifyMemberFaultStopsFleet() + { + var supervisor = CreateStartedSupervisor(); + var statuses = new[] + { + CreateHealthyStatus(1), + new FleetMemberSafetyStatus( + vehicleId: 2, + planId: PlanId, + isStateAvailable: true, + isFaulted: true, + failureCode: 0, + lastAcceptedReportTimeSeconds: 9.9) + }; + + var decision = supervisor.Evaluate( + CreateLayout(), + statuses, + CurrentTimeSeconds); + + AssertStopFromVehicle( + decision, + 2, + "成员故障状态"); + } + + private static void VerifyFailureCodeStopsFleet() + { + var supervisor = CreateStartedSupervisor(); + var statuses = new[] + { + CreateHealthyStatus(1), + new FleetMemberSafetyStatus( + vehicleId: 2, + planId: PlanId, + isStateAvailable: true, + isFaulted: false, + failureCode: 42, + lastAcceptedReportTimeSeconds: 9.9) + }; + + var decision = supervisor.Evaluate( + CreateLayout(), + statuses, + CurrentTimeSeconds); + + AssertStopFromVehicle( + decision, + 2, + "成员故障码"); + } + + private static void VerifyPlanMismatchStopsFleet() + { + var supervisor = CreateStartedSupervisor(); + var statuses = new[] + { + CreateHealthyStatus(1), + new FleetMemberSafetyStatus( + vehicleId: 2, + planId: PlanId - 1, + isStateAvailable: true, + isFaulted: false, + failureCode: 0, + lastAcceptedReportTimeSeconds: 9.9) + }; + + var decision = supervisor.Evaluate( + CreateLayout(), + statuses, + CurrentTimeSeconds); + + AssertStopFromVehicle( + decision, + 2, + "任务编号不一致"); + } + + private static void VerifyStopIsLatchedUntilNewPlanStarts() + { + var supervisor = CreateStartedSupervisor(); + supervisor.Evaluate( + CreateLayout(), + new[] { CreateHealthyStatus(1) }, + CurrentTimeSeconds); + + var latchedDecision = supervisor.Evaluate( + CreateLayout(), + CreateHealthyStatuses(), + CurrentTimeSeconds); + AssertTrue( + latchedDecision.ShouldStop, + "故障恢复后停车决定仍应锁存"); + + supervisor.Start(PlanId + 1); + var recoveredDecision = supervisor.Evaluate( + CreateLayout(), + new[] + { + CreateHealthyStatus(1, PlanId + 1), + CreateHealthyStatus(2, PlanId + 1) + }, + CurrentTimeSeconds); + AssertFalse( + recoveredDecision.ShouldStop, + "开始新任务后应清除旧任务停车锁存"); + } + + private static FleetSafetySupervisor + CreateStartedSupervisor() + { + var supervisor = new FleetSafetySupervisor( + CommunicationTimeoutSeconds); + supervisor.Start(PlanId); + return supervisor; + } + + private static FleetLayout CreateLayout() + { + return new FleetLayout( + new[] + { + new VehicleLayout( + 1, + new Pose2D(-1.0, 0.0, 0.0)), + new VehicleLayout( + 2, + new Pose2D(1.0, 0.0, Math.PI)) + }); + } + + private static IReadOnlyList + CreateHealthyStatuses() + { + return new[] + { + CreateHealthyStatus(1), + CreateHealthyStatus(2) + }; + } + + private static FleetMemberSafetyStatus CreateHealthyStatus( + int vehicleId, + long planId = PlanId) + { + return new FleetMemberSafetyStatus( + vehicleId, + planId, + isStateAvailable: true, + isFaulted: false, + failureCode: 0, + lastAcceptedReportTimeSeconds: 9.9); + } + + private static void AssertStopFromVehicle( + FleetSafetyDecision decision, + int expectedVehicleId, + string scenario) + { + AssertTrue( + decision.ShouldStop, + $"{scenario}时应停车"); + AssertTrue( + decision.SourceVehicleId == expectedVehicleId, + $"{scenario}的来源车辆不正确"); + AssertTrue( + !string.IsNullOrWhiteSpace(decision.Reason), + $"{scenario}应提供停车原因"); + } + + 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); + } + } +} diff --git a/MultiWheelC.Tests/InMemoryFleetTransport.cs b/MultiWheelC.Tests/InMemoryFleetTransport.cs new file mode 100644 index 0000000..c127bb8 --- /dev/null +++ b/MultiWheelC.Tests/InMemoryFleetTransport.cs @@ -0,0 +1,240 @@ +using System; +using System.Collections.Generic; +using MultiWheelC.Fleet; +using MyParking.Shared; + +namespace MultiWheelC.Tests +{ + // 为同一测试进程中的各车辆端点提供共享FIFO消息队列。 + internal sealed class InMemoryFleetTransportNetwork + { + private readonly SharedState _sharedState; + private readonly HashSet _createdEndpointIds = + new HashSet(); + + public InMemoryFleetTransportNetwork( + IReadOnlyList vehicleIds, + int leaderVehicleId) + { + _sharedState = new SharedState( + vehicleIds, + leaderVehicleId); + } + + public InMemoryFleetTransport CreateEndpoint( + int vehicleId) + { + lock (_sharedState.SyncRoot) + { + if (!_sharedState.CommandQueues.ContainsKey( + vehicleId)) + { + throw new ArgumentOutOfRangeException( + nameof(vehicleId), + $"车辆{vehicleId}不属于当前内存车队网络。"); + } + + if (!_createdEndpointIds.Add(vehicleId)) + { + throw new InvalidOperationException( + $"车辆{vehicleId}的内存通信端点已经创建。"); + } + } + + return new InMemoryFleetTransport( + _sharedState, + vehicleId); + } + + internal sealed class SharedState + { + public SharedState( + IReadOnlyList vehicleIds, + int leaderVehicleId) + { + if (vehicleIds == null) + { + throw new ArgumentNullException( + nameof(vehicleIds)); + } + + if (vehicleIds.Count == 0) + { + throw new ArgumentException( + "内存车队网络至少需要一辆车。", + nameof(vehicleIds)); + } + + if (leaderVehicleId <= 0) + { + throw new ArgumentOutOfRangeException( + nameof(leaderVehicleId), + "主车编号必须大于零。"); + } + + CommandQueues = + new Dictionary>(); + + for (var index = 0; + index < vehicleIds.Count; + index++) + { + var vehicleId = vehicleIds[index]; + if (vehicleId <= 0) + { + throw new ArgumentOutOfRangeException( + nameof(vehicleIds), + $"第{index}辆车的编号必须大于零。"); + } + + if (CommandQueues.ContainsKey(vehicleId)) + { + throw new ArgumentException( + $"内存车队网络包含重复车号{vehicleId}。", + nameof(vehicleIds)); + } + + CommandQueues.Add( + vehicleId, + new Queue()); + } + + if (!CommandQueues.ContainsKey( + leaderVehicleId)) + { + throw new ArgumentException( + $"主车{leaderVehicleId}不在车辆列表中。", + nameof(leaderVehicleId)); + } + + LeaderVehicleId = leaderVehicleId; + } + + public object SyncRoot { get; } = new object(); + + public int LeaderVehicleId { get; } + + public Dictionary> + CommandQueues { get; } + + public Queue ReportQueue { get; } = + new Queue(); + } + } + + // 单辆模拟车辆持有的通信端点；只负责消息路由，不解释控制语义。 + internal sealed class InMemoryFleetTransport : IFleetTransport + { + private readonly InMemoryFleetTransportNetwork.SharedState + _sharedState; + private readonly int _localVehicleId; + + internal InMemoryFleetTransport( + InMemoryFleetTransportNetwork.SharedState sharedState, + int localVehicleId) + { + _sharedState = sharedState ?? + throw new ArgumentNullException( + nameof(sharedState)); + _localVehicleId = localVehicleId; + } + + public void SendCommand(FleetCommand command) + { + if (_localVehicleId != + _sharedState.LeaderVehicleId) + { + throw new InvalidOperationException( + "只有主车通信端点可以发送车队命令。"); + } + + lock (_sharedState.SyncRoot) + { + if (command.TargetVehicleId == + FleetProtocol.BroadcastVehicleId) + { + foreach (var pair in + _sharedState.CommandQueues) + { + // 主车本地命令由运行入口直接执行，不通过通信回环。 + if (pair.Key != _localVehicleId) + { + pair.Value.Enqueue(command); + } + } + + return; + } + + if (!_sharedState.CommandQueues.TryGetValue( + command.TargetVehicleId, + out var queue)) + { + throw new ArgumentOutOfRangeException( + nameof(command), + $"目标车辆{command.TargetVehicleId}不存在。"); + } + + queue.Enqueue(command); + } + } + + public void SendReport(FleetMemberReport report) + { + if (report.VehicleId != _localVehicleId) + { + throw new ArgumentException( + $"车辆{_localVehicleId}不能发送属于车辆" + + $"{report.VehicleId}的状态报告。", + nameof(report)); + } + + lock (_sharedState.SyncRoot) + { + _sharedState.ReportQueue.Enqueue(report); + } + } + + public bool TryReceiveCommand( + out FleetCommand command) + { + lock (_sharedState.SyncRoot) + { + var queue = + _sharedState.CommandQueues[_localVehicleId]; + if (queue.Count == 0) + { + command = default; + return false; + } + + command = queue.Dequeue(); + return true; + } + } + + public bool TryReceiveReport( + out FleetMemberReport report) + { + if (_localVehicleId != + _sharedState.LeaderVehicleId) + { + throw new InvalidOperationException( + "只有主车通信端点可以接收成员状态报告。"); + } + + lock (_sharedState.SyncRoot) + { + if (_sharedState.ReportQueue.Count == 0) + { + report = default; + return false; + } + + report = + _sharedState.ReportQueue.Dequeue(); + return true; + } + } + } +} diff --git a/MultiWheelC.Tests/InMemoryFleetTransportTests.cs b/MultiWheelC.Tests/InMemoryFleetTransportTests.cs new file mode 100644 index 0000000..74f3d6a --- /dev/null +++ b/MultiWheelC.Tests/InMemoryFleetTransportTests.cs @@ -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( + () => member2.SendCommand( + CreateCommand(1, sequenceNumber: 1)), + "从车不能发送车队命令"); + AssertThrows( + () => member2.TryReceiveReport(out _), + "从车不能消费全队成员报告"); + AssertThrows( + () => 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( + 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); + } + } +} diff --git a/MultiWheelC.Tests/MultiWheelC.Tests.csproj b/MultiWheelC.Tests/MultiWheelC.Tests.csproj index 03ff0b4..1a5e1ac 100644 --- a/MultiWheelC.Tests/MultiWheelC.Tests.csproj +++ b/MultiWheelC.Tests/MultiWheelC.Tests.csproj @@ -11,4 +11,10 @@ + + + ..\ref\CommonUsage.dll + + + diff --git a/MultiWheelC.Tests/Program.cs b/MultiWheelC.Tests/Program.cs index 3487e4b..4d0c58e 100644 --- a/MultiWheelC.Tests/Program.cs +++ b/MultiWheelC.Tests/Program.cs @@ -42,6 +42,10 @@ namespace MultiWheelC.Tests FleetMemberCommandCorrectorTests.Run(); FleetCoordinatorTests.Run(); FleetPreparationCoordinatorTests.Run(); + FleetMemberAgentTests.Run(); + FleetSafetySupervisorTests.Run(); + InMemoryFleetTransportTests.Run(); + FleetRuntimeTests.Run(); } /// diff --git a/MultiWheelC/Configuration/PilotConfig.ParkingControl.cs b/MultiWheelC/Configuration/PilotConfig.ParkingControl.cs index 281bafa..9f407f5 100644 --- a/MultiWheelC/Configuration/PilotConfig.ParkingControl.cs +++ b/MultiWheelC/Configuration/PilotConfig.ParkingControl.cs @@ -119,7 +119,7 @@ public partial class PilotConfig #region 停车控制-原地自转 [FieldMember(desc = "停车控制：原地自转Kp")] - public float InPlaceRotateKp = 1.03f; + public float InPlaceRotateKp = 1.0f; [FieldMember(desc = "停车控制：原地自转Ki")] public float InPlaceRotateKi = 0f; @@ -141,11 +141,11 @@ public partial class PilotConfig [FieldMember(desc = "停车控制：原地自转最大角速度(deg/s)")] // public float InPlaceRotateMaxSpeed = 47.5f; - public float InPlaceRotateMaxSpeed = 30f; + public float InPlaceRotateMaxSpeed = 45f; [FieldMember(desc = "停车控制：原地自转角加速度(deg/s²)")] // public float InPlaceRotateAcc = 60f; - public float InPlaceRotateAcc = 40f; + public float InPlaceRotateAcc = 45f; [FieldMember(desc = "停车控制：原地自转超时(s)")] public float InPlaceRotateTimeoutSec = 15f; diff --git a/MultiWheelC/Experiments/NewControllerTrackingTests.cs b/MultiWheelC/Experiments/NewControllerTrackingTests.cs index 1adc765..e182caa 100644 --- a/MultiWheelC/Experiments/NewControllerTrackingTests.cs +++ b/MultiWheelC/Experiments/NewControllerTrackingTests.cs @@ -143,6 +143,12 @@ namespace MultiWheelC protected virtual double MotionDirectionInBodyRadians => 0.0; + /// + /// 获取是否由生成后的轨迹自动推导底盘运动坐标系方向。 + /// + protected virtual bool ResolveMotionDirectionFromTrajectory => + false; + /// /// 获取轨迹完成后是否需要将舵轮主动恢复到车头方向。 /// @@ -244,7 +250,9 @@ namespace MultiWheelC Trajectory = trajectory, StateProvider = _stateProvider, MotionDirectionInBodyRadians = - MotionDirectionInBodyRadians, + ResolveMotionDirectionFromTrajectory + ? (double?)null + : MotionDirectionInBodyRadians, ReturnWheelsForwardAfterCompletion = ReturnWheelsForwardAfterCompletion, CycleObserver = controller => @@ -505,6 +513,12 @@ namespace MultiWheelC protected override double MotionDirectionInBodyRadians => Math.PI / 4.0; + /// + /// 只用45°定义参考轨迹，底盘β由轨迹切线和车身参考航向自动推导。 + /// + protected override bool ResolveMotionDirectionFromTrajectory => + true; + /// /// 蟹行轨迹正常完成后主动将四个舵轮恢复到车头方向。 /// @@ -586,6 +600,12 @@ namespace MultiWheelC protected virtual double MotionDirectionInBodyRadians => 0.0; + /// + /// 获取是否由生成后的轨迹自动推导底盘运动坐标系方向。 + /// + protected virtual bool ResolveMotionDirectionFromTrajectory => + false; + /// /// 获取轨迹完成后是否需要将舵轮主动恢复到车头方向。 /// @@ -698,7 +718,9 @@ namespace MultiWheelC Trajectory = trajectory, StateProvider = _stateProvider, MotionDirectionInBodyRadians = - MotionDirectionInBodyRadians, + ResolveMotionDirectionFromTrajectory + ? (double?)null + : MotionDirectionInBodyRadians, ReturnWheelsForwardAfterCompletion = ReturnWheelsForwardAfterCompletion, CycleObserver = controller => @@ -936,6 +958,12 @@ namespace MultiWheelC protected override double MotionDirectionInBodyRadians => Math.PI / 4.0; + /// + /// 只用45°定义参考轨迹，底盘β由整段轨迹自动推导并检查一致性。 + /// + protected override bool ResolveMotionDirectionFromTrajectory => + true; + /// /// 蟹行组合轨迹正常完成后主动将四个舵轮恢复到车头方向。 /// diff --git a/MultiWheelC/Fleet/FleetCoordinator.cs b/MultiWheelC/Fleet/FleetCoordinator.cs index fd690f9..8217d0f 100644 --- a/MultiWheelC/Fleet/FleetCoordinator.cs +++ b/MultiWheelC/Fleet/FleetCoordinator.cs @@ -2,7 +2,7 @@ using System; using System.Collections.Generic; using MultiWheelC.Trajectory; using MyParking.Shared; - +// 负责运动中：每周期计算每辆车的速度命令 namespace MultiWheelC.Fleet { // 主车单周期车队协调结果，不表示通信或成员底盘执行结果。 @@ -173,6 +173,10 @@ namespace MultiWheelC.Fleet public string LastFailureReason { get; private set; } + // 运动前成员β准备必须与车队控制器采用同一个车队运动方向。 + public double MotionDirectionInFleetRadians => + _fleetController.MotionDirectionInFleetRadians; + public void Start( FleetLayout layout, Trajectory2D trajectory) diff --git a/MultiWheelC/Fleet/FleetMemberAgent.cs b/MultiWheelC/Fleet/FleetMemberAgent.cs index d66294a..f7946e9 100644 --- a/MultiWheelC/Fleet/FleetMemberAgent.cs +++ b/MultiWheelC/Fleet/FleetMemberAgent.cs @@ -30,6 +30,8 @@ namespace MultiWheelC.Fleet private readonly double _alignmentStableSeconds; private double _alignedDurationSeconds; + private double? _lastAcceptedCommandTimeSeconds; + private double? _commandDeadlineSeconds; /// 创建绑定到一辆多舵轮底盘的成员车执行器。 public FleetMemberAgent( @@ -81,6 +83,13 @@ namespace MultiWheelC.Fleet public string LastFailureReason { get; private set; } + // 使用从车本机单调时钟记录，不依赖主车或Detour时间戳。 + public double? LastAcceptedCommandTimeSeconds => + _lastAcceptedCommandTimeSeconds; + + public double? CommandDeadlineSeconds => + _commandDeadlineSeconds; + /// 停车并开始准备本车固定β滚动运动系。 public bool BeginRollingPreparation( long planId, @@ -158,8 +167,18 @@ namespace MultiWheelC.Fleet } /// 在主车确认全队Ready后激活本车已经准备好的运动方式。 - public bool Activate(long planId) + public bool Activate( + long planId, + double commandReceivedTimeSeconds, + double validForSeconds) { + NumericGuard.EnsureFiniteNonNegative( + commandReceivedTimeSeconds, + nameof(commandReceivedTimeSeconds)); + NumericGuard.EnsureFinitePositive( + validForSeconds, + nameof(validForSeconds)); + if (planId != CurrentPlanId) { LastFailureReason = @@ -169,7 +188,9 @@ namespace MultiWheelC.Fleet if (State == FleetMemberAgentState.Active) { - return true; + // 重复激活只允许幂等确认，不能替代周期运动命令延长车辆运动时间。 + return UpdateCommandWatchdog( + commandReceivedTimeSeconds); } if (State != FleetMemberAgentState.Ready || @@ -227,6 +248,9 @@ namespace MultiWheelC.Fleet } State = FleetMemberAgentState.Active; + AcceptCommandDeadline( + commandReceivedTimeSeconds, + validForSeconds); LastFailureReason = string.Empty; return true; } @@ -235,12 +259,20 @@ namespace MultiWheelC.Fleet public bool Execute( long planId, FleetMemberCommand command, + double commandReceivedTimeSeconds, + double validForSeconds, TimeSpan? interval = null) { ValidatePlanId(planId); NumericGuard.EnsureFinite( command.TwistInVehicleBody, nameof(command)); + NumericGuard.EnsureFiniteNonNegative( + commandReceivedTimeSeconds, + nameof(commandReceivedTimeSeconds)); + NumericGuard.EnsureFinitePositive( + validForSeconds, + nameof(validForSeconds)); if (planId != CurrentPlanId) { @@ -262,6 +294,13 @@ namespace MultiWheelC.Fleet "成员车尚未激活，不能执行速度命令。"); } + // 先检查上一条命令是否已经过期，禁止失联后由迟到命令自动恢复运动。 + if (!UpdateCommandWatchdog( + commandReceivedTimeSeconds)) + { + return false; + } + if (!IsCommandCompatibleWithPreparation( command.TwistInVehicleBody)) { @@ -289,10 +328,59 @@ namespace MultiWheelC.Fleet return Fail(exception.Message); } + AcceptCommandDeadline( + commandReceivedTimeSeconds, + validForSeconds); LastFailureReason = string.Empty; return true; } + // 运行循环即使没有收到新命令也必须调用本方法，超时后会本地停车并锁存Faulted。 + public bool UpdateCommandWatchdog( + double currentTimeSeconds) + { + NumericGuard.EnsureFiniteNonNegative( + currentTimeSeconds, + nameof(currentTimeSeconds)); + + if (State == FleetMemberAgentState.Faulted) + { + return false; + } + + if (State != FleetMemberAgentState.Active) + { + return true; + } + + if (!_lastAcceptedCommandTimeSeconds.HasValue || + !_commandDeadlineSeconds.HasValue) + { + return Fail( + "成员车已经激活，但本地命令看门狗尚未初始化。"); + } + + if (currentTimeSeconds < + _lastAcceptedCommandTimeSeconds.Value) + { + return Fail( + "成员车本地单调时钟发生倒退，无法继续校验命令时效。"); + } + + if (currentTimeSeconds <= + _commandDeadlineSeconds.Value) + { + return true; + } + + var commandAgeSeconds = + currentTimeSeconds - + _lastAcceptedCommandTimeSeconds.Value; + return Fail( + "成员车等待主车有效命令超时，" + + $"最近一次命令距今{commandAgeSeconds:F3}s。"); + } + /// 正常取消当前任务并立即停止驱动轮。 public void Stop() { @@ -302,6 +390,7 @@ namespace MultiWheelC.Fleet CurrentPlanId = 0; MotionDirectionInBodyRadians = 0.0; _alignedDurationSeconds = 0.0; + ClearCommandWatchdog(); LastFailureReason = string.Empty; } @@ -323,6 +412,7 @@ namespace MultiWheelC.Fleet State = FleetMemberAgentState.Preparing; LastFailureReason = string.Empty; _alignedDurationSeconds = 0.0; + ClearCommandWatchdog(); var accepted = mode == FleetMemberPreparationMode.Rolling @@ -423,6 +513,29 @@ namespace MultiWheelC.Fleet return false; } + private void AcceptCommandDeadline( + double commandReceivedTimeSeconds, + double validForSeconds) + { + var commandDeadlineSeconds = + commandReceivedTimeSeconds + + validForSeconds; + NumericGuard.EnsureFinite( + commandDeadlineSeconds, + nameof(validForSeconds)); + + _lastAcceptedCommandTimeSeconds = + commandReceivedTimeSeconds; + _commandDeadlineSeconds = + commandDeadlineSeconds; + } + + private void ClearCommandWatchdog() + { + _lastAcceptedCommandTimeSeconds = null; + _commandDeadlineSeconds = null; + } + /// 锁存成员车故障并立即清零驱动轮速度。 private bool Fail(string reason) { diff --git a/MultiWheelC/Fleet/FleetPreparationCoordinator.cs b/MultiWheelC/Fleet/FleetPreparationCoordinator.cs index 0eecb5b..df88533 100644 --- a/MultiWheelC/Fleet/FleetPreparationCoordinator.cs +++ b/MultiWheelC/Fleet/FleetPreparationCoordinator.cs @@ -2,6 +2,7 @@ using System; using System.Collections.Generic; using System.Collections.ObjectModel; using MyParking.Shared; +// 负责运动前：所有车辆舵轮是否准备完成 namespace MultiWheelC.Fleet { diff --git a/MultiWheelC/Fleet/FleetRuntime.cs b/MultiWheelC/Fleet/FleetRuntime.cs new file mode 100644 index 0000000..81766b5 --- /dev/null +++ b/MultiWheelC/Fleet/FleetRuntime.cs @@ -0,0 +1,1010 @@ +using System; +using System.Collections.Generic; +using MultiWheelC.StateEstimation; +using MultiWheelC.Trajectory; +using MyParking.Shared; + +namespace MultiWheelC.Fleet +{ + /// 表示本机车队任务运行入口的生命周期阶段。 + public enum FleetRuntimeState + { + Idle = 0, + Preparing = 1, + Ready = 2, + Active = 3, + Completed = 4, + Faulted = 5 + } + + /// 在主车或从车上串联固定布局滚动车队任务。 + public sealed class FleetRuntime + { + private const int RuntimeFailureCode = 1; + private const int FaultStopReasonCode = 1; + + private readonly IFleetTransport _transport; + private readonly FleetMemberAgent _localAgent; + private readonly IVehicleStateProvider _localStateProvider; + private readonly FleetPreparationCoordinator _preparationCoordinator; + private readonly FleetCoordinator _fleetCoordinator; + private readonly FleetSafetySupervisor _safetySupervisor; + private readonly double _commandValidForSeconds; + private readonly double _preparationTimeoutSeconds; + private readonly Dictionary _reports = + new Dictionary(); + private readonly Dictionary _lastReportSequences = + new Dictionary(); + + private FleetLayout _activeLayout; + private VehicleState? _lastLocalState; + private double? _planStartTimeSeconds; + private long _nextCommandSequence; + private long _nextReportSequence; + private long _lastReceivedCommandSequence; + private long _lastAppliedCommandSequence; + + /// 创建本车运行入口；主车必须额外提供三个主车侧组件。 + public FleetRuntime( + int selfVehicleId, + int leaderVehicleId, + IFleetTransport transport, + FleetMemberAgent localAgent, + IVehicleStateProvider localStateProvider, + FleetPreparationCoordinator preparationCoordinator = null, + FleetCoordinator fleetCoordinator = null, + FleetSafetySupervisor safetySupervisor = null, + double commandValidForSeconds = 0.5, + double preparationTimeoutSeconds = 10.0) + { + if (selfVehicleId <= 0) + { + throw new ArgumentOutOfRangeException(nameof(selfVehicleId)); + } + + if (leaderVehicleId <= 0) + { + throw new ArgumentOutOfRangeException(nameof(leaderVehicleId)); + } + + _transport = transport ?? + throw new ArgumentNullException(nameof(transport)); + _localAgent = localAgent ?? + throw new ArgumentNullException(nameof(localAgent)); + _localStateProvider = localStateProvider ?? + throw new ArgumentNullException(nameof(localStateProvider)); + NumericGuard.EnsureFinitePositive( + commandValidForSeconds, + nameof(commandValidForSeconds)); + NumericGuard.EnsureFinitePositive( + preparationTimeoutSeconds, + nameof(preparationTimeoutSeconds)); + + if (_localAgent.VehicleId != selfVehicleId) + { + throw new ArgumentException( + "本车执行器车号与运行入口车号不一致。", + nameof(localAgent)); + } + + SelfVehicleId = selfVehicleId; + LeaderVehicleId = leaderVehicleId; + _preparationCoordinator = preparationCoordinator; + _fleetCoordinator = fleetCoordinator; + _safetySupervisor = safetySupervisor; + _commandValidForSeconds = commandValidForSeconds; + _preparationTimeoutSeconds = preparationTimeoutSeconds; + + if (IsLeader && + (_preparationCoordinator == null || + _fleetCoordinator == null || + _safetySupervisor == null)) + { + throw new ArgumentException( + "主车必须提供准备协调器、车队协调器和安全监督器。"); + } + + State = FleetRuntimeState.Idle; + LastFailureReason = string.Empty; + } + + public int SelfVehicleId { get; } + + public int LeaderVehicleId { get; } + + public bool IsLeader => SelfVehicleId == LeaderVehicleId; + + public FleetRuntimeState State { get; private set; } + + public long CurrentPlanId { get; private set; } + + public long LastAppliedCommandSequence => + _lastAppliedCommandSequence; + + public string LastFailureReason { get; private set; } + + public FleetCoordinationCycleOutput LastCoordinationOutput + { + get; + private set; + } + + /// 由主车启动一次使用当前控制器固定β的滚动轨迹任务。 + public bool StartRollingPlan( + long planId, + FleetLayout layout, + Trajectory2D trajectory) + { + EnsureLeader(); + if (planId <= 0) + { + throw new ArgumentOutOfRangeException(nameof(planId)); + } + + if (layout == null) + { + throw new ArgumentNullException(nameof(layout)); + } + + if (trajectory == null) + { + throw new ArgumentNullException(nameof(trajectory)); + } + + if (!layout.TryGetVehicle(SelfVehicleId, out _)) + { + throw new ArgumentException( + "主车不在当前车队布局中。", + nameof(layout)); + } + + if (State == FleetRuntimeState.Preparing || + State == FleetRuntimeState.Active) + { + LastFailureReason = "当前车队任务尚未结束。"; + return false; + } + + ResetLeaderForNewPlan(); + CurrentPlanId = planId; + _activeLayout = layout; + State = FleetRuntimeState.Preparing; + + try + { + _fleetCoordinator.Start(layout, trajectory); + _preparationCoordinator.StartRollingPreparation( + planId, + layout, + _fleetCoordinator.MotionDirectionInFleetRadians); + _safetySupervisor.Start(planId); + + foreach (var target in _preparationCoordinator.Targets) + { + var command = NewCommand( + target.VehicleId, + FleetCommandKind.PrepareRolling, + target.MotionDirectionInBodyRadians, + Twist2D.Zero); + + if (target.VehicleId == SelfVehicleId) + { + if (!_localAgent.BeginRollingPreparation( + planId, + target.MotionDirectionInBodyRadians)) + { + return FaultLeader( + LocalFailure("主车舵轮准备失败。")); + } + + AcceptLocalCommand(command.SequenceNumber); + } + else + { + _transport.SendCommand(command); + } + } + + return true; + } + catch (Exception exception) + { + return FaultLeader( + "启动车队任务失败：" + exception.Message); + } + } + + /// 执行一个本机调度周期，并返回更新后的运行阶段。 + public FleetRuntimeState Update( + double currentTimeSeconds, + double deltaTimeSeconds) + { + NumericGuard.EnsureFiniteNonNegative( + currentTimeSeconds, + nameof(currentTimeSeconds)); + NumericGuard.EnsureFinitePositive( + deltaTimeSeconds, + nameof(deltaTimeSeconds)); + + return IsLeader + ? UpdateLeader(currentTimeSeconds, deltaTimeSeconds) + : UpdateMember(currentTimeSeconds, deltaTimeSeconds); + } + + /// 正常取消当前任务；主车同时向其余成员广播停止。 + public void Stop(string reason = "") + { + if (IsLeader && CurrentPlanId > 0) + { + BroadcastStop(reasonCode: 0); + _preparationCoordinator.Cancel(); + _fleetCoordinator.Cancel(); + _safetySupervisor.Reset(); + } + + _localAgent.Stop(); + _activeLayout = null; + CurrentPlanId = FleetProtocol.NoActivePlanId; + State = FleetRuntimeState.Idle; + _planStartTimeSeconds = null; + LastCoordinationOutput = null; + LastFailureReason = reason ?? string.Empty; + } + + private FleetRuntimeState UpdateLeader( + double currentTimeSeconds, + double deltaTimeSeconds) + { + if (State != FleetRuntimeState.Preparing && + State != FleetRuntimeState.Active) + { + return State; + } + + _planStartTimeSeconds ??= currentTimeSeconds; + if (!ReceiveReports(currentTimeSeconds)) + { + return State; + } + + if (!TickLocalAgent(currentTimeSeconds, deltaTimeSeconds)) + { + FaultLeader(LocalFailure("主车本地执行失败。")); + return State; + } + + if (!TryReadLocalState(out var localState)) + { + FaultLeader("主车本地状态不可用。" ); + return State; + } + + StoreLocalReport(localState.Value, currentTimeSeconds); + + if (State == FleetRuntimeState.Preparing) + { + UpdatePreparationBarrier(); + if (_preparationCoordinator.State == + FleetPreparationCoordinatorState.Faulted) + { + FaultLeader(_preparationCoordinator.LastFailureReason); + return State; + } + + if (currentTimeSeconds - _planStartTimeSeconds.Value > + _preparationTimeoutSeconds) + { + FaultLeader("车队舵轮准备超时，未能全员Ready。" ); + return State; + } + } + + if (!CheckLeaderSafety(currentTimeSeconds)) + { + return State; + } + + if (State == FleetRuntimeState.Preparing && + !ActivateFleet(currentTimeSeconds)) + { + return State; + } + + if (State == FleetRuntimeState.Active) + { + RunCoordination(currentTimeSeconds, deltaTimeSeconds); + } + + return State; + } + + private FleetRuntimeState UpdateMember( + double currentTimeSeconds, + double deltaTimeSeconds) + { + ReceiveCommands(currentTimeSeconds); + if (!TickLocalAgent(currentTimeSeconds, deltaTimeSeconds)) + { + if (State != FleetRuntimeState.Faulted) + { + FaultMember(LocalFailure("成员车本地执行失败。")); + } + } + + var hasState = TryReadLocalState(out var localState); + if (!hasState && CurrentPlanId > 0 && + State != FleetRuntimeState.Faulted) + { + FaultMember("成员车本地状态不可用。" ); + } + + SendLocalReport(localState, hasState, currentTimeSeconds); + return State; + } + + private bool TickLocalAgent( + double currentTimeSeconds, + double deltaTimeSeconds) + { + if (State == FleetRuntimeState.Faulted) + { + return false; + } + + if (_localAgent.State == FleetMemberAgentState.Preparing) + { + _localAgent.UpdatePreparation(deltaTimeSeconds); + } + + if (_localAgent.State == FleetMemberAgentState.Active && + !_localAgent.UpdateCommandWatchdog(currentTimeSeconds)) + { + return false; + } + + if (_localAgent.State == FleetMemberAgentState.Faulted) + { + return false; + } + + if (!IsLeader) + { + State = ToRuntimeState(_localAgent.State); + } + + return true; + } + + private bool TryReadLocalState(out VehicleState? state) + { + try + { + if (_localStateProvider.TryGetState(out var value)) + { + _lastLocalState = value; + state = value; + return true; + } + } + catch (Exception exception) + { + LastFailureReason = + "读取本车状态失败：" + exception.Message; + } + + state = null; + return false; + } + + private bool ReceiveReports(double currentTimeSeconds) + { + try + { + while (_transport.TryReceiveReport(out var report)) + { + if (report.VehicleId == SelfVehicleId || + _activeLayout == null || + !_activeLayout.TryGetVehicle(report.VehicleId, out _) || + report.PlanId != CurrentPlanId) + { + continue; + } + + ValidateReport(report); + if (_lastReportSequences.TryGetValue( + report.VehicleId, + out var sequence) && + report.SequenceNumber <= sequence) + { + continue; + } + + _lastReportSequences[report.VehicleId] = + report.SequenceNumber; + _reports[report.VehicleId] = + new AcceptedReport(report, currentTimeSeconds); + } + + return true; + } + catch (Exception exception) + { + return FaultLeader( + "接收成员报告失败：" + exception.Message); + } + } + + private void StoreLocalReport( + VehicleState state, + double currentTimeSeconds) + { + _reports[SelfVehicleId] = new AcceptedReport( + CreateReport(state, isStateAvailable: true), + currentTimeSeconds); + } + + private void UpdatePreparationBarrier() + { + foreach (var accepted in _reports.Values) + { + var report = accepted.Report; + _preparationCoordinator.ReportMemberStatus( + new FleetMemberPreparationStatus( + CurrentPlanId, + report.VehicleId, + ToAgentState(report.State), + report.State == FleetMemberState.Faulted + ? $"成员车故障码{report.FailureCode}。" + : string.Empty)); + } + } + + private bool CheckLeaderSafety(double currentTimeSeconds) + { + var statuses = new List(); + foreach (var accepted in _reports.Values) + { + var report = accepted.Report; + statuses.Add(new FleetMemberSafetyStatus( + report.VehicleId, + report.PlanId, + report.IsStateAvailable, + report.State == FleetMemberState.Faulted, + report.FailureCode, + accepted.ReceivedTimeSeconds)); + } + + var graceExpired = + currentTimeSeconds - _planStartTimeSeconds.Value > + _safetySupervisor.CommunicationTimeoutSeconds; + if (statuses.Count != _activeLayout.VehicleCount && + !graceExpired) + { + return true; + } + + var decision = _safetySupervisor.Evaluate( + _activeLayout, + statuses, + currentTimeSeconds); + return !decision.ShouldStop || + FaultLeader(decision.Reason); + } + + private bool ActivateFleet(double currentTimeSeconds) + { + if (_preparationCoordinator.State != + FleetPreparationCoordinatorState.ReadyToActivate) + { + return false; + } + + if (!_preparationCoordinator.TryAuthorizeActivation( + CurrentPlanId)) + { + return FaultLeader("无法授权全队激活。" ); + } + + var command = NewCommand( + FleetProtocol.BroadcastVehicleId, + FleetCommandKind.Activate, + 0.0, + Twist2D.Zero); + if (!_localAgent.Activate( + CurrentPlanId, + currentTimeSeconds, + command.ValidForSeconds)) + { + return FaultLeader(LocalFailure("主车激活失败。")); + } + + AcceptLocalCommand(command.SequenceNumber); + try + { + _transport.SendCommand(command); + State = FleetRuntimeState.Active; + return true; + } + catch (Exception exception) + { + return FaultLeader( + "广播激活命令失败：" + exception.Message); + } + } + + private void RunCoordination( + double currentTimeSeconds, + double deltaTimeSeconds) + { + var result = _fleetCoordinator.ExecuteCycle( + BuildMemberStates(), + currentTimeSeconds, + deltaTimeSeconds, + out var output); + LastCoordinationOutput = output; + + if (result == FleetCoordinationCycleResult.CommandGenerated || + result == FleetCoordinationCycleResult.WaitingForState) + { + Dispatch(output.MemberCommands, currentTimeSeconds); + return; + } + + if (result == FleetCoordinationCycleResult.Completed) + { + BroadcastStop(reasonCode: 0); + _localAgent.Stop(); + _preparationCoordinator.Cancel(); + _safetySupervisor.Reset(); + State = FleetRuntimeState.Completed; + return; + } + + var reason = string.IsNullOrWhiteSpace(output.Reason) + ? _fleetCoordinator.LastFailureReason + : output.Reason; + FaultLeader(string.IsNullOrWhiteSpace(reason) + ? "车队协调器未生成可执行命令。" + : reason); + } + + private bool Dispatch( + IReadOnlyList commands, + double currentTimeSeconds) + { + if (commands == null || + commands.Count != _activeLayout.VehicleCount) + { + return FaultLeader("成员命令数量与布局不一致。" ); + } + + try + { + foreach (var memberCommand in commands) + { + var command = NewCommand( + memberCommand.VehicleId, + FleetCommandKind.Motion, + 0.0, + memberCommand.TwistInVehicleBody); + if (memberCommand.VehicleId == SelfVehicleId) + { + if (!_localAgent.Execute( + CurrentPlanId, + memberCommand, + currentTimeSeconds, + command.ValidForSeconds)) + { + return FaultLeader( + LocalFailure("主车执行速度命令失败。")); + } + + AcceptLocalCommand(command.SequenceNumber); + } + else + { + _transport.SendCommand(command); + } + } + + return true; + } + catch (Exception exception) + { + return FaultLeader( + "分发成员命令失败：" + exception.Message); + } + } + + private void ReceiveCommands(double currentTimeSeconds) + { + try + { + while (_transport.TryReceiveCommand(out var command)) + { + ApplyCommand(command, currentTimeSeconds); + } + } + catch (Exception exception) + { + FaultMember( + "接收或执行主车命令失败：" + exception.Message); + } + } + + private void ApplyCommand( + FleetCommand command, + double currentTimeSeconds) + { + if (command.TargetVehicleId != + FleetProtocol.BroadcastVehicleId && + command.TargetVehicleId != SelfVehicleId) + { + return; + } + + ValidateCommand(command); + var isPreparation = + command.Kind == FleetCommandKind.PrepareRolling || + command.Kind == FleetCommandKind.PrepareSpin; + + if (isPreparation && command.PlanId != CurrentPlanId) + { + if (State == FleetRuntimeState.Preparing || + State == FleetRuntimeState.Ready || + State == FleetRuntimeState.Active) + { + return; + } + + CurrentPlanId = command.PlanId; + _lastReceivedCommandSequence = 0; + _lastAppliedCommandSequence = 0; + LastFailureReason = string.Empty; + } + + if (command.PlanId != CurrentPlanId || + command.SequenceNumber <= _lastReceivedCommandSequence) + { + return; + } + + _lastReceivedCommandSequence = command.SequenceNumber; + bool accepted; + switch (command.Kind) + { + case FleetCommandKind.PrepareRolling: + accepted = _localAgent.BeginRollingPreparation( + command.PlanId, + command.MotionDirectionInBodyRadians); + break; + case FleetCommandKind.PrepareSpin: + accepted = + _localAgent.BeginSpinPreparation(command.PlanId); + break; + case FleetCommandKind.Activate: + accepted = _localAgent.Activate( + command.PlanId, + currentTimeSeconds, + command.ValidForSeconds); + break; + case FleetCommandKind.Motion: + accepted = _localAgent.Execute( + command.PlanId, + new FleetMemberCommand( + SelfVehicleId, + command.TwistInVehicleBody), + currentTimeSeconds, + command.ValidForSeconds); + break; + case FleetCommandKind.Stop: + _localAgent.Stop(); + _lastAppliedCommandSequence = command.SequenceNumber; + CurrentPlanId = FleetProtocol.NoActivePlanId; + State = FleetRuntimeState.Idle; + LastFailureReason = string.Empty; + return; + default: + accepted = false; + break; + } + + if (!accepted) + { + FaultMember(LocalFailure( + $"成员车拒绝{command.Kind}命令。")); + return; + } + + _lastAppliedCommandSequence = command.SequenceNumber; + State = ToRuntimeState(_localAgent.State); + } + + private void SendLocalReport( + VehicleState? state, + bool isStateAvailable, + double currentTimeSeconds) + { + try + { + var value = state ?? _lastLocalState ?? + new VehicleState( + currentTimeSeconds, + Pose2D.Identity, + Twist2D.Zero, + hasValidVelocityEstimate: false); + _transport.SendReport( + CreateReport(value, isStateAvailable)); + } + catch (Exception exception) + { + FaultMember( + "发送成员报告失败：" + exception.Message); + } + } + + private FleetMemberReport CreateReport( + VehicleState state, + bool isStateAvailable) + { + return new FleetMemberReport( + SelfVehicleId, + CurrentPlanId, + NextReportSequence(), + state.SampleTimestampSeconds, + state.PoseInWorld, + state.TwistInWorld, + isStateAvailable, + isStateAvailable && state.HasValidVelocityEstimate, + ToProtocolState(), + _lastAppliedCommandSequence, + State == FleetRuntimeState.Faulted + ? RuntimeFailureCode + : 0); + } + + private List BuildMemberStates() + { + var states = new List( + _activeLayout.VehicleCount); + foreach (var vehicle in _activeLayout.Vehicles) + { + var report = _reports[vehicle.VehicleId].Report; + states.Add(new FleetMemberStateSample( + report.VehicleId, + report.SampleTimestampSeconds, + report.PoseInCommonWorld, + report.TwistAtVehicleOriginInCommonWorld, + report.IsStateAvailable, + report.HasValidVelocityEstimate)); + } + + return states; + } + + private FleetCommand NewCommand( + int targetVehicleId, + FleetCommandKind kind, + double motionDirectionInBodyRadians, + Twist2D twist, + int reasonCode = 0) + { + return new FleetCommand( + CurrentPlanId, + NextCommandSequence(), + targetVehicleId, + kind, + motionDirectionInBodyRadians, + twist, + _commandValidForSeconds, + reasonCode); + } + + private bool FaultLeader(string reason) + { + var preservedReason = string.IsNullOrWhiteSpace(reason) + ? "车队运行入口发生未说明故障。" + : reason; + if (CurrentPlanId > 0) + { + BroadcastStop(FaultStopReasonCode); + } + + _localAgent.Stop(); + State = FleetRuntimeState.Faulted; + LastFailureReason = preservedReason; + return false; + } + + private void FaultMember(string reason) + { + var preservedReason = string.IsNullOrWhiteSpace(reason) + ? "成员车运行入口发生未说明故障。" + : reason; + _localAgent.Stop(); + State = FleetRuntimeState.Faulted; + LastFailureReason = preservedReason; + } + + private void BroadcastStop(int reasonCode) + { + try + { + _transport.SendCommand(NewCommand( + FleetProtocol.BroadcastVehicleId, + FleetCommandKind.Stop, + 0.0, + Twist2D.Zero, + reasonCode)); + } + catch + { + // 本车仍立即停车，失联成员由各自命令看门狗兜底。 + } + } + + private void ResetLeaderForNewPlan() + { + _localAgent.Stop(); + _preparationCoordinator.Cancel(); + _fleetCoordinator.Cancel(); + _safetySupervisor.Reset(); + _reports.Clear(); + _lastReportSequences.Clear(); + _activeLayout = null; + _lastLocalState = null; + _planStartTimeSeconds = null; + _lastReceivedCommandSequence = 0; + _lastAppliedCommandSequence = 0; + LastCoordinationOutput = null; + LastFailureReason = string.Empty; + } + + private void AcceptLocalCommand(long sequenceNumber) + { + _lastReceivedCommandSequence = sequenceNumber; + _lastAppliedCommandSequence = sequenceNumber; + } + + private long NextCommandSequence() + { + if (++_nextCommandSequence <= 0) + { + throw new InvalidOperationException("主车命令序号耗尽。" ); + } + + return _nextCommandSequence; + } + + private long NextReportSequence() + { + if (++_nextReportSequence <= 0) + { + throw new InvalidOperationException("成员报告序号耗尽。" ); + } + + return _nextReportSequence; + } + + private string LocalFailure(string fallback) + { + return string.IsNullOrWhiteSpace(_localAgent.LastFailureReason) + ? fallback + : _localAgent.LastFailureReason; + } + + private void EnsureLeader() + { + if (!IsLeader) + { + throw new InvalidOperationException( + "只有主车可以启动完整车队任务。"); + } + } + + private FleetMemberState ToProtocolState() + { + if (State == FleetRuntimeState.Faulted) + { + return FleetMemberState.Faulted; + } + + return _localAgent.State switch + { + FleetMemberAgentState.Idle => FleetMemberState.Idle, + FleetMemberAgentState.Preparing => FleetMemberState.Preparing, + FleetMemberAgentState.Ready => FleetMemberState.Ready, + FleetMemberAgentState.Active => FleetMemberState.Active, + FleetMemberAgentState.Faulted => FleetMemberState.Faulted, + _ => throw new InvalidOperationException("本车状态无效。") + }; + } + + private static FleetRuntimeState ToRuntimeState( + FleetMemberAgentState state) + { + return state switch + { + FleetMemberAgentState.Idle => FleetRuntimeState.Idle, + FleetMemberAgentState.Preparing => FleetRuntimeState.Preparing, + FleetMemberAgentState.Ready => FleetRuntimeState.Ready, + FleetMemberAgentState.Active => FleetRuntimeState.Active, + FleetMemberAgentState.Faulted => FleetRuntimeState.Faulted, + _ => throw new ArgumentOutOfRangeException(nameof(state)) + }; + } + + private static FleetMemberAgentState ToAgentState( + FleetMemberState state) + { + return state switch + { + FleetMemberState.Idle => FleetMemberAgentState.Idle, + FleetMemberState.Preparing => FleetMemberAgentState.Preparing, + FleetMemberState.Ready => FleetMemberAgentState.Ready, + FleetMemberState.Active => FleetMemberAgentState.Active, + FleetMemberState.Faulted => FleetMemberAgentState.Faulted, + _ => throw new ArgumentOutOfRangeException(nameof(state)) + }; + } + + private static void ValidateCommand(FleetCommand command) + { + if (command.PlanId <= 0 || command.SequenceNumber <= 0) + { + throw new ArgumentException("车队命令任务号或序号无效。" ); + } + + if (!Enum.IsDefined(typeof(FleetCommandKind), command.Kind)) + { + throw new ArgumentException("车队命令类型无效。" ); + } + + NumericGuard.EnsureFinite( + command.MotionDirectionInBodyRadians, + nameof(command)); + NumericGuard.EnsureFinite( + command.TwistInVehicleBody, + nameof(command)); + NumericGuard.EnsureFinitePositive( + command.ValidForSeconds, + nameof(command)); + } + + private static void ValidateReport(FleetMemberReport report) + { + if (report.VehicleId <= 0 || report.PlanId <= 0 || + report.SequenceNumber <= 0 || + report.LastAppliedCommandSequence < 0 || + !Enum.IsDefined(typeof(FleetMemberState), report.State)) + { + throw new ArgumentException("成员状态报告字段无效。" ); + } + + NumericGuard.EnsureFiniteNonNegative( + report.SampleTimestampSeconds, + nameof(report)); + NumericGuard.EnsureFinite( + report.PoseInCommonWorld, + nameof(report)); + NumericGuard.EnsureFinite( + report.TwistAtVehicleOriginInCommonWorld, + nameof(report)); + } + + private readonly struct AcceptedReport + { + public AcceptedReport( + FleetMemberReport report, + double receivedTimeSeconds) + { + Report = report; + ReceivedTimeSeconds = receivedTimeSeconds; + } + + public FleetMemberReport Report { get; } + + public double ReceivedTimeSeconds { get; } + } + } +} diff --git a/MultiWheelC/Fleet/FleetSafetySupervisor.cs b/MultiWheelC/Fleet/FleetSafetySupervisor.cs new file mode 100644 index 0000000..7028fe3 --- /dev/null +++ b/MultiWheelC/Fleet/FleetSafetySupervisor.cs @@ -0,0 +1,286 @@ +using System; +using System.Collections.Generic; +using MyParking.Shared; + +namespace MultiWheelC.Fleet +{ + // 主车已经接收并接受的一辆成员车安全状态。 + public readonly struct FleetMemberSafetyStatus + { + public FleetMemberSafetyStatus( + int vehicleId, + long planId, + bool isStateAvailable, + bool isFaulted, + int failureCode, + double lastAcceptedReportTimeSeconds) + { + if (vehicleId <= 0) + { + throw new ArgumentOutOfRangeException( + nameof(vehicleId), + "成员车编号必须大于零。"); + } + + if (planId < 0) + { + throw new ArgumentOutOfRangeException( + nameof(planId), + "任务编号不能为负数。"); + } + + NumericGuard.EnsureFiniteNonNegative( + lastAcceptedReportTimeSeconds, + nameof(lastAcceptedReportTimeSeconds)); + + VehicleId = vehicleId; + PlanId = planId; + IsStateAvailable = isStateAvailable; + IsFaulted = isFaulted; + FailureCode = failureCode; + LastAcceptedReportTimeSeconds = + lastAcceptedReportTimeSeconds; + } + + public int VehicleId { get; } + + public long PlanId { get; } + + public bool IsStateAvailable { get; } + + public bool IsFaulted { get; } + + // 零表示成员车没有报告结构化故障。 + public int FailureCode { get; } + + // 使用主车本地单调时钟，不能直接填写从车上传的时间戳。 + public double LastAcceptedReportTimeSeconds { get; } + } + + // 一次安全检查的结果；ShouldStop可直接作为是否停车的判断标识。 + public readonly struct FleetSafetyDecision + { + internal FleetSafetyDecision( + bool shouldStop, + int sourceVehicleId, + string reason) + { + ShouldStop = shouldStop; + SourceVehicleId = sourceVehicleId; + Reason = reason ?? string.Empty; + } + + public bool ShouldStop { get; } + + // 零表示原因属于整个车队，而不是某一辆成员车。 + public int SourceVehicleId { get; } + + public string Reason { get; } + } + + // 检查成员通信和健康状态，并锁存需要整队停车的首个原因。 + public sealed class FleetSafetySupervisor + { + private readonly double _communicationTimeoutSeconds; + + private long _activePlanId; + private FleetSafetyDecision _latchedDecision; + + public FleetSafetySupervisor( + double communicationTimeoutSeconds) + { + NumericGuard.EnsureFinitePositive( + communicationTimeoutSeconds, + nameof(communicationTimeoutSeconds)); + + _communicationTimeoutSeconds = + communicationTimeoutSeconds; + Reset(); + } + + public double CommunicationTimeoutSeconds => + _communicationTimeoutSeconds; + + public long ActivePlanId => _activePlanId; + + public bool IsActive => _activePlanId > 0; + + public bool IsStopLatched => + _latchedDecision.ShouldStop; + + public FleetSafetyDecision LastDecision => + _latchedDecision; + + // 开始一次新任务，同时清除上一任务留下的停车锁存。 + public void Start(long planId) + { + if (planId <= 0) + { + throw new ArgumentOutOfRangeException( + nameof(planId), + "活动任务编号必须大于零。"); + } + + _activePlanId = planId; + _latchedDecision = CreateContinueDecision(); + } + + // 返回ShouldStop；本类只负责判定，实际停车由后续运行入口执行。 + public FleetSafetyDecision Evaluate( + FleetLayout layout, + IReadOnlyList memberStatuses, + double currentTimeSeconds) + { + if (layout == null) + { + throw new ArgumentNullException(nameof(layout)); + } + + if (memberStatuses == null) + { + throw new ArgumentNullException( + nameof(memberStatuses)); + } + + NumericGuard.EnsureFiniteNonNegative( + currentTimeSeconds, + nameof(currentTimeSeconds)); + + if (!IsActive) + { + return new FleetSafetyDecision( + true, + 0, + "车队安全监督器尚未启动活动任务。"); + } + + if (IsStopLatched) + { + return _latchedDecision; + } + + var statusesByVehicleId = + new Dictionary(); + + for (var index = 0; + index < memberStatuses.Count; + index++) + { + var status = memberStatuses[index]; + + // 非当前编队成员的状态不参与本次任务安全判定。 + if (!layout.TryGetVehicle( + status.VehicleId, + out _)) + { + continue; + } + + if (statusesByVehicleId.ContainsKey( + status.VehicleId)) + { + return LatchStop( + status.VehicleId, + $"成员车{status.VehicleId}存在重复状态报告。"); + } + + statusesByVehicleId.Add( + status.VehicleId, + status); + } + + for (var index = 0; + index < layout.Vehicles.Count; + index++) + { + var vehicleId = + layout.Vehicles[index].VehicleId; + + if (!statusesByVehicleId.TryGetValue( + vehicleId, + out var status)) + { + return LatchStop( + vehicleId, + $"未收到成员车{vehicleId}的状态报告。"); + } + + if (status.PlanId != _activePlanId) + { + return LatchStop( + vehicleId, + $"成员车{vehicleId}报告的任务编号" + + $"{status.PlanId}与当前任务" + + $"{_activePlanId}不一致。"); + } + + if (status.LastAcceptedReportTimeSeconds > + currentTimeSeconds) + { + return LatchStop( + vehicleId, + $"成员车{vehicleId}的主车接收时间晚于当前时间。"); + } + + var reportAgeSeconds = + currentTimeSeconds - + status.LastAcceptedReportTimeSeconds; + if (reportAgeSeconds > + _communicationTimeoutSeconds) + { + return LatchStop( + vehicleId, + $"成员车{vehicleId}通信超时，" + + $"最近有效报告距今" + + $"{reportAgeSeconds:F3}s。"); + } + + if (status.IsFaulted || + status.FailureCode != 0) + { + return LatchStop( + vehicleId, + $"成员车{vehicleId}报告故障，" + + $"故障码为{status.FailureCode}。"); + } + + if (!status.IsStateAvailable) + { + return LatchStop( + vehicleId, + $"成员车{vehicleId}状态不可用。"); + } + } + + _latchedDecision = CreateContinueDecision(); + return _latchedDecision; + } + + // 结束当前任务并清除锁存；未开始新任务前Evaluate仍会要求停车。 + public void Reset() + { + _activePlanId = 0; + _latchedDecision = CreateContinueDecision(); + } + + private FleetSafetyDecision LatchStop( + int sourceVehicleId, + string reason) + { + _latchedDecision = new FleetSafetyDecision( + true, + sourceVehicleId, + reason); + return _latchedDecision; + } + + private static FleetSafetyDecision + CreateContinueDecision() + { + return new FleetSafetyDecision( + false, + 0, + string.Empty); + } + } +} diff --git a/MultiWheelC/Fleet/IFleetTransport.cs b/MultiWheelC/Fleet/IFleetTransport.cs new file mode 100644 index 0000000..506909b --- /dev/null +++ b/MultiWheelC/Fleet/IFleetTransport.cs @@ -0,0 +1,16 @@ +using MyParking.Shared; + +namespace MultiWheelC.Fleet +{ + // 隔离车队运行逻辑与具体无线、串口或内存传输实现。 + public interface IFleetTransport + { + void SendCommand(FleetCommand command); + + void SendReport(FleetMemberReport report); + + bool TryReceiveCommand(out FleetCommand command); + + bool TryReceiveReport(out FleetMemberReport report); + } +} diff --git a/MultiWheelC/Movements/TrajectoryTrackingMovement.cs b/MultiWheelC/Movements/TrajectoryTrackingMovement.cs index 4787916..3aa0584 100644 --- a/MultiWheelC/Movements/TrajectoryTrackingMovement.cs +++ b/MultiWheelC/Movements/TrajectoryTrackingMovement.cs @@ -21,6 +21,11 @@ namespace MultiWheelC public sealed class TrajectoryTrackingMovement : MovementDefinition { + private const double ReferenceSpeedDeadbandMetersPerSecond = + 1e-6; + private const double FixedMotionDirectionToleranceRadians = + 3.0 * Math.PI / 180.0; + /// /// 获取或设置本次动作需要跟踪的世界坐标系轨迹。 /// @@ -43,9 +48,18 @@ namespace MultiWheelC public Action CycleObserver; /// - /// 获取或设置本动作运动坐标系X轴在车体系中的方向，单位为rad；0表示车头方向。 + /// 获取或设置本动作运动坐标系X轴在车体系中的方向，单位为rad；为空时从轨迹自动推导。 /// - public double MotionDirectionInBodyRadians; + public double? MotionDirectionInBodyRadians = 0.0; + + /// + /// 获取本次执行最终采用的运动坐标系方向，动作尚未开始时为空。 + /// + public double? ResolvedMotionDirectionInBodyRadians + { + get; + private set; + } /// /// 获取或设置轨迹正常完成后是否停车并将舵轮主动恢复到车头方向。 @@ -270,6 +284,12 @@ namespace MultiWheelC config.ParkingExecutionTimeoutSeconds; ValidateParameters(executionTimeoutSeconds); + var motionDirectionInBodyRadians = + MotionDirectionInBodyRadians ?? + ResolveFixedMotionDirectionInBodyRadians( + Trajectory); + ResolvedMotionDirectionInBodyRadians = + motionDirectionInBodyRadians; var chassis = PilotDefinition.Chassis as MultiWheelChassis; @@ -283,7 +303,7 @@ namespace MultiWheelC new PrepareWheelsForward { DirectionRadians = - MotionDirectionInBodyRadians + motionDirectionInBodyRadians }; foreach (var keepRunning in wheelPreparation.Get()) { @@ -306,7 +326,7 @@ namespace MultiWheelC PilotDefinition.Self.CarNum); adapter.ActivateMotionFrame( - MotionDirectionInBodyRadians); + motionDirectionInBodyRadians); var stateProvider = StateProvider ?? @@ -348,7 +368,7 @@ namespace MultiWheelC new GcpCommandExecutor( adapter, maximumGcpAngleRateRadiansPerSecond, - MotionDirectionInBodyRadians); + motionDirectionInBodyRadians); Controller = new ParkingGeometricController( stateProvider, @@ -366,7 +386,7 @@ namespace MultiWheelC maximumTerminalApproachSpeedMetersPerSecond, stanleyCurvaturePreviewSeconds, stanleyMaximumCurvaturePreviewMeters, - MotionDirectionInBodyRadians); + motionDirectionInBodyRadians); var clock = Stopwatch.StartNew(); var previousCycleSeconds = @@ -478,9 +498,12 @@ namespace MultiWheelC "新版轨迹跟踪动作没有设置Trajectory。"); } - NumericGuard.EnsureFinite( - MotionDirectionInBodyRadians, - nameof(MotionDirectionInBodyRadians)); + if (MotionDirectionInBodyRadians.HasValue) + { + NumericGuard.EnsureFinite( + MotionDirectionInBodyRadians.Value, + nameof(MotionDirectionInBodyRadians)); + } if (double.IsNaN(executionTimeoutSeconds) || double.IsInfinity(executionTimeoutSeconds) || @@ -491,5 +514,114 @@ namespace MultiWheelC "轨迹跟踪超时时间必须是正有限值。"); } } + + /// + /// 根据轨迹切线、参考车身航向和速度符号推导整段轨迹共同使用的固定运动方向。 + /// + private static double ResolveFixedMotionDirectionInBodyRadians( + Trajectory2D trajectory) + { + double? resolvedDirectionRadians = null; + + for (var index = 0; + index < trajectory.Count - 1; + index++) + { + var segmentStart = trajectory[index]; + var segmentEnd = trajectory[index + 1]; + var travelDirection = ResolveSegmentTravelDirection( + segmentStart.ReferenceSpeedMetersPerSecond, + segmentEnd.ReferenceSpeedMetersPerSecond, + index); + if (travelDirection == 0.0) + { + continue; + } + + var tangentYawRadians = Math.Atan2( + segmentEnd.PoseInWorld.YMeters - + segmentStart.PoseInWorld.YMeters, + segmentEnd.PoseInWorld.XMeters - + segmentStart.PoseInWorld.XMeters); + var positiveMotionAxisYawRadians = + travelDirection > 0.0 + ? tangentYawRadians + : AngleMath.NormalizeRadians( + tangentYawRadians + Math.PI); + var referenceBodyYawRadians = + AngleMath.LerpRadians( + segmentStart.PoseInWorld.YawRadians, + segmentEnd.PoseInWorld.YawRadians, + 0.5); + var candidateDirectionRadians = + AngleMath.ShortestDifferenceRadians( + positiveMotionAxisYawRadians, + referenceBodyYawRadians); + + if (!resolvedDirectionRadians.HasValue) + { + resolvedDirectionRadians = + candidateDirectionRadians; + continue; + } + + var directionDifferenceRadians = Math.Abs( + AngleMath.ShortestDifferenceRadians( + candidateDirectionRadians, + resolvedDirectionRadians.Value)); + if (directionDifferenceRadians > + FixedMotionDirectionToleranceRadians) + { + throw new InvalidOperationException( + "轨迹无法由一个固定运动坐标系执行：" + + $"第{index + 1}段需要的方向与起始方向相差" + + $"{AngleMath.RadiansToDegrees(directionDifferenceRadians):F2}°。" + + "请拆分轨迹，或显式指定并验证MotionDirectionInBodyRadians。"); + } + } + + if (!resolvedDirectionRadians.HasValue) + { + throw new InvalidOperationException( + "轨迹没有非零参考速度线段，无法自动确定运动坐标系方向。"); + } + + return resolvedDirectionRadians.Value; + } + + /// + /// 从相邻轨迹点的有符号参考速度确定该线段的执行方向。 + /// + private static double ResolveSegmentTravelDirection( + double startSpeedMetersPerSecond, + double endSpeedMetersPerSecond, + int segmentStartIndex) + { + var hasStartDirection = + Math.Abs(startSpeedMetersPerSecond) > + ReferenceSpeedDeadbandMetersPerSecond; + var hasEndDirection = + Math.Abs(endSpeedMetersPerSecond) > + ReferenceSpeedDeadbandMetersPerSecond; + + if (hasStartDirection && + hasEndDirection && + Math.Sign(startSpeedMetersPerSecond) != + Math.Sign(endSpeedMetersPerSecond)) + { + throw new InvalidOperationException( + $"轨迹第{segmentStartIndex + 1}段内参考速度发生正负切换，" + + "无法自动确定固定运动坐标系；请在零速点拆分动作段。"); + } + + if (hasStartDirection) + { + return Math.Sign(startSpeedMetersPerSecond); + } + + return hasEndDirection + ? Math.Sign(endSpeedMetersPerSecond) + : 0.0; + } } } diff --git a/MultiWheelC/build/Clumsy/CommonUsage.dll b/MultiWheelC/build/Clumsy/CommonUsage.dll index 2c0496a..d284e2e 100644 Binary files a/MultiWheelC/build/Clumsy/CommonUsage.dll and b/MultiWheelC/build/Clumsy/CommonUsage.dll differ diff --git a/MultiWheelC/build/Clumsy/MultiWheelC.dll b/MultiWheelC/build/Clumsy/MultiWheelC.dll index 59c03a9..666bccd 100644 Binary files a/MultiWheelC/build/Clumsy/MultiWheelC.dll and b/MultiWheelC/build/Clumsy/MultiWheelC.dll differ diff --git a/MultiWheelC/build/Clumsy/MultiWheelC.pdb b/MultiWheelC/build/Clumsy/MultiWheelC.pdb index 0106fc9..d62166c 100644 Binary files a/MultiWheelC/build/Clumsy/MultiWheelC.pdb and b/MultiWheelC/build/Clumsy/MultiWheelC.pdb differ diff --git a/Shared/Fleet/FleetProtocol.cs b/Shared/Fleet/FleetProtocol.cs index bf6ca47..b7504c4 100644 --- a/Shared/Fleet/FleetProtocol.cs +++ b/Shared/Fleet/FleetProtocol.cs @@ -1 +1,141 @@ -// 只定义无线报文的数据结构、版本和字段 \ No newline at end of file +// 四舵轮共同搬运协议 + +namespace MyParking.Shared +{ + /// 定义车队无线协议的公共常量。 + public static class FleetProtocol + { + public const int CurrentVersion = 1; + public const int BroadcastVehicleId = 0; + public const long NoActivePlanId = 0; + public const long NoAppliedCommandSequence = 0; + } + + /// 表示成员车本地的车队任务执行阶段。 + public enum FleetMemberState + { + Idle = 0, + Preparing = 1, + Ready = 2, + Active = 3, + Faulted = 4 + } + + /// 表示主车要求成员执行的动作。 + public enum FleetCommandKind + { + PrepareRolling = 1, + PrepareSpin = 2, + Activate = 3, + Motion = 4, + Stop = 5 + } + + /// 成员车周期上报给主车的状态快照，同时承担心跳和命令确认。 + public readonly struct FleetMemberReport + { + public FleetMemberReport( + int vehicleId, + long planId, + long sequenceNumber, + double sampleTimestampSeconds, + Pose2D poseInCommonWorld, + Twist2D twistAtVehicleOriginInCommonWorld, + bool isStateAvailable, + bool hasValidVelocityEstimate, + FleetMemberState state, + long lastAppliedCommandSequence, + int failureCode = 0) + { + VehicleId = vehicleId; + PlanId = planId; + SequenceNumber = sequenceNumber; + SampleTimestampSeconds = sampleTimestampSeconds; + PoseInCommonWorld = poseInCommonWorld; + TwistAtVehicleOriginInCommonWorld = + twistAtVehicleOriginInCommonWorld; + IsStateAvailable = isStateAvailable; + HasValidVelocityEstimate = + hasValidVelocityEstimate; + State = state; + LastAppliedCommandSequence = + lastAppliedCommandSequence; + FailureCode = failureCode; + } + + public int VehicleId { get; } + + // 零表示车辆当前不属于活动任务。 + public long PlanId { get; } + + // 本车上报流中单调递增，用于丢弃乱序旧报文。 + public long SequenceNumber { get; } + + // 本车单调时钟的采样时刻，通信层负责换算到主车时间轴。 + public double SampleTimestampSeconds { get; } + + // 位姿必须已经转换到所有成员约定一致的公共世界坐标系。 + public Pose2D PoseInCommonWorld { get; } + + public Twist2D TwistAtVehicleOriginInCommonWorld { get; } + + public bool IsStateAvailable { get; } + + public bool HasValidVelocityEstimate { get; } + + public FleetMemberState State { get; } + + // 零表示尚未执行任何主车命令。 + public long LastAppliedCommandSequence { get; } + + // 零表示没有结构化故障码。 + public int FailureCode { get; } + } + + /// 主车向指定成员或全队下发的一条车队任务命令。 + public readonly struct FleetCommand + { + public FleetCommand( + long planId, + long sequenceNumber, + int targetVehicleId, + FleetCommandKind kind, + double motionDirectionInBodyRadians, + Twist2D twistInVehicleBody, + double validForSeconds, + int reasonCode = 0) + { + PlanId = planId; + SequenceNumber = sequenceNumber; + TargetVehicleId = targetVehicleId; + Kind = kind; + MotionDirectionInBodyRadians = + motionDirectionInBodyRadians; + TwistInVehicleBody = twistInVehicleBody; + ValidForSeconds = validForSeconds; + ReasonCode = reasonCode; + } + + public long PlanId { get; } + + // 主车命令流中单调递增，成员据此拒绝乱序旧命令。 + public long SequenceNumber { get; } + + // 零表示广播，正数表示指定成员车。 + public int TargetVehicleId { get; } + + public FleetCommandKind Kind { get; } + + // 仅PrepareRolling使用，单位rad，车体系X轴到运动X轴逆时针为正。 + public double MotionDirectionInBodyRadians { get; } + + // 仅Motion使用，采用目标成员车体系。 + public Twist2D TwistInVehicleBody { get; } + + // 从成员本机收到消息时开始计时，超时后必须停车。 + public double ValidForSeconds { get; } + + // 零表示没有结构化停止或故障原因。 + public int ReasonCode { get; } + } +} diff --git a/docs/architecture.md b/docs/architecture.md index b3ad42a..ebdb961 100644 --- a/docs/architecture.md +++ b/docs/architecture.md @@ -89,7 +89,7 @@ MovementTest / MotionPlanExecutor `TrajectoryTrackingMovement` 默认从 `PilotDefinition.Conf` 读取车辆级参数，同时保留少量动作级覆盖字段；横向控制器可通过 `LateralControllerFactory` 替换，纵向控制器当前固定创建为 `PidLongitudinalController`。 -## 车队组件与尚未贯通的执行链 +## 车队组件与首版运行链 ```text 夹紧且静止时的成员世界位姿快照 @@ -109,13 +109,27 @@ MovementTest / MotionPlanExecutor 车队动作主要滚动方向β_fleet → FleetPreparationCoordinator（换算每车β_i并等待全员Ready） → FleetMemberAgent（本车停车准备、舵轮到位、激活、Execute/Stop） + +主车FleetCommand + → IFleetTransport + → 从车FleetMemberAgent + +从车FleetMemberReport + → IFleetTransport + → 主车状态缓存 / FleetSafetySupervisor + +FleetRuntime + ├─ 主车：准备屏障 → 状态/安全 → FleetCoordinator → 本车执行/远端分发 + └─ 从车：接收命令 → FleetMemberAgent → 本地看门狗 → 状态上报 ``` `FleetLayoutCapture` 只负责固定布局的几何计算：车队原点X/Y取成员车体中心的算术平均，车队Yaw取主车Yaw，再把各成员世界位姿反变换为 `VehicleLayout.PoseInFleet`。它不读取通信或Detour，也不负责静止/夹紧确认、时间对齐和布局激活。 -上述类目前是可以独立构造和测试的组件，并没有正式的车队任务运行入口把两条链串起来。缺少的外层需要负责布局原子激活、成员状态实际采集与时间对齐、准备/激活状态机、每周期协调、安全门控、成员命令分发、任务完成与取消。`FleetCoordinator` 生成零速或故障结果不等于实车已经停车；只有运行层把结果送到各车 `FleetMemberAgent.Execute()` 或 `Stop()` 后才会影响底盘。 +`Shared/Fleet/FleetProtocol.cs` 已定义最小任务命令和成员报告值类型，`IFleetTransport` 已把运行逻辑与无线串口或内存队列隔离；测试项目中的 `InMemoryFleetTransport` 只用于无通信硬件验证，不进入正式部署。`FleetSafetySupervisor` 在主车侧按成员报告接收时间、状态和故障锁存整队停车决定，`FleetMemberAgent.UpdateCommandWatchdog()` 在每辆车本地按命令有效期独立停车，避免通信中断时只依赖主车广播停止。 -实际部署为每车独立电脑，因此主车还需要状态/命令通信，从车需要本地命令超时看门狗。无线串口初始化可以后接，但消息契约、任务号、心跳/有效期和本地失联停车语义必须在运行层接入前明确。 +`MultiWheelC/Fleet/FleetRuntime.cs` 已贯通首版固定布局滚动任务：同一类型按 `selfVehicleId`/`leaderVehicleId` 区分角色；主车执行β准备、全员Ready、统一激活、状态缓存、安全判定、周期协调、本车直接执行和远端分发；从车执行任务/序列校验、舵轮准备、激活、速度命令、本地看门狗和状态上报。完成、通信/状态/成员故障和取消都会由运行层转换为本车停车及可达成员停止命令，失联成员最终由本地命令有效期兜底。 + +当前端到端链路只通过测试项目的内存传输验证，尚未接入C层正式 `Movement/Experiment`、实际成员状态采集、无线串口和真实跨机时间换算。实际部署仍是每车独立电脑；主车也是普通成员，其本车命令不经传输层回环。无线实现不能改变已经定义的任务号、序列号、命令有效期和本地失联停车语义。 ## 状态数据流 diff --git a/docs/decisions.md b/docs/decisions.md index 315db7c..4e011f8 100644 --- a/docs/decisions.md +++ b/docs/decisions.md @@ -112,24 +112,34 @@ - `PathTrackingContext` 只携带受控刚体的车体系速度、速度有效性、轨迹投影和周期控制量，不依赖 `VehicleState` 或 `FleetState`。 - `PathTrackingCore` 集中实现投影连续性、保护/终点策略、曲率预瞄、横纵向控制和GCP分配；`ParkingGeometricController` 与 `FleetController` 分别组合该核心，负责各自的状态适配和输出边界，不通过继承复制控制流程。 - `FleetController` 第一版只闭环车队虚拟中心，并支持固定 `β_fleet`：实际纵向速度沿β投影，GCP结果从运动坐标系旋转到车队坐标系后形成车队原点处的 `FleetMotionCommand`；成员速度继续由 `FleetKinematics.Decompose()` 确定性分解。 -- `FleetStateEstimator` 已根据固定 `FleetLayout` 和时间对齐目标时刻反算、融合车队中心，并输出每车 `FleetMemberLayoutError`；成员样本的实际采集和跨电脑时间处理仍属于外层接收链。 -- `FleetCoordinator` 已串联状态估计、虚拟中心控制、布局误差警告区间内的统一速度缩放、刚体分解和 `FleetMemberCommandCorrector` 小范围纠偏；越过停止阈值时返回故障和零速成员命令，但尚无正式运行层把该结果送到实车。 -- `FleetPreparationCoordinator` 已按 `beta_i = beta_fleet - theta_i` 生成成员准备目标，并通过任务号和全员Ready形成统一激活屏障；`FleetMemberAgent` 已封装本车停车准备、舵轮到位确认、激活、命令校验和底盘执行。两者尚未接入同一个车队任务生命周期，也未接入跨电脑通信。 +- `FleetStateEstimator` 已根据固定 `FleetLayout` 和时间对齐目标时刻反算、融合车队中心，并输出每车 `FleetMemberLayoutError`；当前第一版先做候选中心两两一致性检查，再对X/Y等权平均、对Yaw做圆周平均。正负候选偏差的公共分量进入车队中心，相对分量仍保留在成员布局误差中，不会因平均而消失。成员样本的实际采集和跨电脑时间处理仍属于外层接收链。 +- `FleetCoordinator` 已串联状态估计、虚拟中心控制、布局误差警告区间内的统一速度缩放、刚体分解和 `FleetMemberCommandCorrector` 小范围纠偏；`FleetRuntime` 已把周期结果转换为本车直接执行和远端成员命令。 +- `FleetPreparationCoordinator` 已按 `beta_i = beta_fleet - theta_i` 生成成员准备目标，并通过任务号和全员Ready形成统一激活屏障；`FleetMemberAgent` 负责本车舵轮准备、激活、命令校验和底盘执行；`FleetRuntime` 将二者接入同一固定布局滚动任务生命周期。 - 控制算法的可替换性继续由 `ILateralController`、`ILongitudinalController` 组合注入；状态源、通信、底盘发送和成员协调不进入纯核心。 - `MultiWheelC.Tests` 已覆盖8个Stanley前进/倒车符号场景、8个车队控制周期场景、6个布局采集场景和4个刚体分解场景；统一构建与打包通过。 +### 16. 车队通信保持最小值契约，失联停车采用主车与本车双层保护 + +- `Shared/Fleet/FleetProtocol.cs` 只保存 `FleetCommand`、`FleetMemberReport`、枚举和协议常量，不绑定停车机器人控制器、串口实现或复杂消息类层次；无线字节帧和校验留在具体传输实现。 +- `IFleetTransport` 是运行层唯一传输边界；当前 `InMemoryFleetTransport` 只在测试项目中模拟定向命令、广播和成员报告，后续无线实现替换该接口而不改车队算法。 +- 主车 `FleetSafetySupervisor` 依据主车本地接收时间、成员状态和故障锁存整队停车决定；每辆车的 `FleetMemberAgent` 依据本机单调时间和命令有效期独立看门狗停车。`FleetRuntime` 已执行本车停止并向可达成员下发停止，两层保护仍不能互相替代。 +- 同一 `FleetRuntime` 部署到所有车辆，角色由显式 `selfVehicleId` 与 `leaderVehicleId` 决定，不把固定车号硬编码为主车；主车也是成员，本车命令直接执行，不要求传输层回环。 + ## 已经确认但尚未实施 - 路线顺序：先完成单车闭环和停车功能验证，再正式实施多车通信、编队和协同控制。来源：`README.md`。 -- 当前多车的布局、状态估计、中心控制、刚体分解、成员纠偏、β准备屏障和本车执行代理均已有代码组件，但尚未形成可运行的多车链路。仍缺少车队任务运行入口、布局原子激活、成员状态实际采集/时间对齐、跨电脑通信、命令分发和可实际触发停车的安全执行链。 +- 转舵系统辨识应对四个轮组分别使用相同激励，输入采用实际差速转舵命令（`TotalDiff`或左右轮实际发送命令之差），输出采用实际舵角；若只用目标角到实际角，会把当前PID包含在闭环模型中，更换PID后该模型不能继续代表转舵机构。控制阶段优先保留四个独立PID实例和内部状态、共用一套参数，并依据四个健康轮组中最不利的动态设计稳定裕量；只有硬件健康且公共参数仍无法兼顾时，才评估配置化的逐轮小幅校准，不在代码中硬编码某个轮位特例。 +- 当前多车的固定布局滚动任务已形成内存通信可运行链路。仍缺少C层正式动作入口、布局原子激活、成员状态实际采集、跨机时间换算、公共世界坐标验证和无线传输实现。 +- 单车 `MultiWheelC/StateEstimation` 继续负责Detour重复帧、跳变候选、轮速短时预测和任务坐标连续化；车队层不复制这套原始定位处理，只消费经过本车校验的成员状态，并负责跨车时间对齐、固定布局反算、成员一致性检查和中心融合。成员状态进入融合前仍必须确认处于同一公共坐标系；各车独立的 `_controlFromDetour` 连续化变换是否会造成跨车基准差异，属于通信/状态接收契约必须验证的事项。 +- 第一版保留当前保守的等权车队中心融合；任务生命周期和内存零速运行链已经贯通。公共坐标系与主车时间轴语义确认并取得静止/低速双车日志后，再按数据增加车队历史预测、逐成员创新门控、健康降级和Huber等鲁棒加权；不在缺少Detour协方差时提前实现协方差加权或Covariance Intersection。 - 布局生命周期区分夹紧前后的语义：夹紧前的预设布局只用于引导车辆就位；车辆夹紧且静止后，应同步取得同一世界坐标系下的成员位姿，调用 `FleetLayoutCapture` 创建新的不可变布局，再由上层协调器原子激活。共同搬运期间的相对位姿变化属于状态误差，不能通过修改 `FleetLayout` 吸收；松开车辆后清除激活布局。实际数据采集和激活接口尚未实施。 - 多车共同搬运不能只闭环车队中心：整体位姿误差与成员相对布局误差必须分开估计和约束，否则成员误差可能相互抵消而使平均中心看似正确。 - 计划采用分层职责：车队控制器产生参考点 `FleetTwist`，分配层依据成员 `VehicleLayout` 计算每车真实车体系 `BodyTwist`，单车层继续负责β变换、GCP和本车四轮解算。 - 第一版采用确定性的虚拟刚体速度分配，不先引入QP/HQP：若成员在车队系中的固定布局为位置 `(x_i,y_i)`、朝向 `theta_i`，则成员中心在车队系中的速度为 `(Vx-omega*y_i, Vy+omega*x_i, omega)`，再通过 `R(-theta_i)` 转到本车体系后交给 `SendBodyTwist()`。QP/HQP只在需要同时调整车队参考速度、处理成员能力差异、松弛约束或严格任务优先级时再引入。 -- “按状态最差车辆协调速度”第一步已经对成员相对布局误差实现警告阈值至停止阈值之间的统一速度缩放。成员报警、通信超时、夹紧异常、状态持续不可用等整队停车条件仍需由真实运行层统一执行；时间戳、心跳、命令有效期和从车本地超时停车属于第一版安全契约，延迟预测补偿可以后续增加。 +- “按状态最差车辆协调速度”第一步已经对成员相对布局误差实现警告阈值至停止阈值之间的统一速度缩放。通信超时、状态不可用、成员故障和命令有效期已经进入运行链；成员报警/夹紧信号来源、可恢复降级策略和实车阈值仍待接入，延迟预测补偿可以后续增加。 - 虚拟车队使用固定在车队坐标系中的对称前后GCP，把横向控制结果转换成车队原点 `FleetTwist`；这些点不是物理轮轴，也不直接参与单车四轮解算。横向控制器与GCP到Twist转换必须使用同一控制点半径；具体车辆级配置值和实车验证仍待完成。 - 每辆成员车都应作为反馈来源，但反馈职责必须分层：成员Detour位姿用于融合车队整体位姿和检查相对布局，单车轮速/舵角用于确认命令执行偏差，电机电流、扭矩或力传感信息用于负载与内力监控。相对位姿接近目标并不能证明没有内力，因此不能只依靠刚性连接或位姿误差判断负载均衡。 -- 第一版不把每车β作为复杂优化变量：`FleetController` 已支持固定的车队主要滚动方向 `beta_fleet`（如正常0°、斜行45°、横移90°）及其控制坐标转换；`FleetPreparationCoordinator` 已按布局换算成员 `beta_i`，并采用180°等效轴保持在方便的本地表示范围。β是单车执行坐标系，不改变刚体分配得到的真实车体系 `BodyTwist`，也不会让各车命令数值相同。当前缺的是把停车预对齐、全员Ready和统一激活接入真实运行链；只有出现复杂布局、整段方向变化、限位余量或频繁反号问题时，才增加轨迹级β候选搜索。 +- 第一版不把每车β作为复杂优化变量：`FleetController` 支持固定车队主要滚动方向 `beta_fleet`，`FleetPreparationCoordinator` 按布局换算成员 `beta_i` 并采用180°等效轴，`FleetRuntime` 已将停车预对齐、全员Ready和统一激活接入运行链。β是单车执行坐标系，不改变刚体分配得到的真实车体系 `BodyTwist`；只有出现复杂布局、整段方向变化、限位余量或频繁反号问题时，才增加轨迹级β候选搜索。 - 旧版参考项目采用固定双车布局：各车由 `carWorld ∘ layout⁻¹` 反推车队中心，再对位置和圆周航向求平均；路径控制器以该虚拟中心跟踪轨迹。同时它可按 `fleetTarget ∘ layout_i` 生成每车理想位姿，并叠加Detour布局纠偏和邻车两腿检测纠偏，因此并非只控制平均中心。来源：`原版停车机器人/parkingrobot/ClumsyPilot/PilotDefinition.cs`、`ChassisController.cs`。 - 旧版 `SetOriginBias(layoutX, layoutY, layoutTh)` 是把各车真实轮子统一表达在车队虚拟坐标系中，属于固定编队布局变换。旧版联动显式区分常规、蟹行和绕车队中心旋转三类模式；蟹行角可由动作或遥控给出任意值（`FleetCrabWalk` 默认45°），并在运动前以零速度对齐舵轮、运行时使用180°等效和轮速反号，但没有根据整段轨迹和每车约束自主求解β的统一规划过程。给定简单蟹行动作时，它与新版固定β可能产生相同的实际轮子姿态和车辆运动。 - 旧版自动 `FleetCurveWalk`、`FleetCrabWalk` 会先以零速度下发初始GCP角，等待成员新鲜、布局正确、命令可行、舵轮到位和从车应用新序列后才开始运动；原地旋转通过 `RotateWheelsAligned` 和 `FleetMotionReleased` 做整队释放。普通手动入口仍有 `SendMotion` 本车舵轮未对齐时速度置零的门控，但不保证与自动动作相同的车队级同步屏障。 @@ -140,7 +150,7 @@ ## 待评估 -- 多车共同搬运时的车队参考点与固定GCP距离、任意成员布局、加权/异常值鲁棒的车队位姿估计、队形误差闭环，以及 `FleetMotionCommand → 每车Twist2D` 的具体分配与限幅算法。 +- 多车共同搬运时的车队参考点与固定GCP距离、逐成员能力/命令可行性限幅，以及带时间预测和异常成员隔离的鲁棒车队位姿融合；当前任意成员布局、确定性刚体分解和小范围队形误差闭环已有第一版实现。 - 确定性刚体分配验证完成后，再评估集中式单步QP/HQP：安全和刚体可行性应作为硬约束或更高层级，相对布局、中心跟踪、平滑与能耗依次降低优先级；严格HQP不能仅靠单个加权QP的大权重近似。短时域MPC及舵轮动态延迟预测属于更后续阶段。 - 负载共享和内力监控可用信号、阈值、降级与停车策略；当前项目尚未建立可确认的力/扭矩闭环。 - 正式轨迹规划层与 `Trajectory2D` 的接入格式；当前 `TestTrajectoryFactory` 仅用于实验。 diff --git a/docs/interfaces.md b/docs/interfaces.md index 8157797..fc969f7 100644 --- a/docs/interfaces.md +++ b/docs/interfaces.md @@ -47,6 +47,28 @@ 该接口是纯几何计算，调用者必须在外部保证成员位姿处于同一世界坐标系，并完成夹紧、静止、数据新鲜度和时间对齐检查；布局的存储与原子激活也不属于该类。 +## 车队通信与安全接口 + +`Shared/Fleet/FleetProtocol.cs` 只定义与具体无线模块无关的值类型： + +- `FleetCommand`：任务号、主车命令序号、目标车号、命令类型、成员β、成员车体系速度、有效期和原因码。目标车号0表示广播。 +- `FleetMemberReport`：车号、任务号、本车报告序号、来源采样时间、公共世界系位姿/速度、状态有效性、本地任务阶段、最近执行命令序号和故障码；它同时承担心跳和命令确认。 +- `FleetCommandKind` 当前覆盖滚动准备、自转准备、激活、运动和停止；`FleetMemberState` 覆盖空闲、准备、就绪、活动和故障。 + +`MultiWheelC/Fleet/IFleetTransport.cs` 仅提供 `SendCommand`、`SendReport`、`TryReceiveCommand` 和 `TryReceiveReport`。它不负责串口初始化、字节序列化、任务/序列校验、安全判定或时钟同步；测试项目的 `InMemoryFleetTransport` 按车号路由FIFO消息，广播只送从车，主车本地命令由 `FleetRuntime` 直接执行。 + +时间语义分层：报告的 `SampleTimestampSeconds` 是来源车的采样时刻，真实跨机接收层必须换算到主车时间轴后才能用于 `FleetMemberStateSample` 对齐；`FleetMemberSafetyStatus.LastAcceptedReportTimeSeconds` 必须使用主车本地单调接收时间；`FleetMemberAgent` 的命令接收时间和截止时间必须使用本车本地单调时钟。Detour `tick` 不能直接替代这三类时间。 + +`FleetSafetySupervisor` 只在主车侧检查成员缺失、任务不一致、通信超时、状态不可用和故障，并锁存首个 `ShouldStop` 原因；它不直接操作底盘。`FleetMemberAgent.UpdateCommandWatchdog()` 是每车本地最后一道失联保护，活动状态下命令过期或本地时钟倒退会立即 `StopImmediately()` 并进入 `Faulted`。运行循环即使没有收到新命令也必须周期调用该方法。 + +### `FleetRuntime` + +- 构造时传入本车ID、主车ID、`IFleetTransport`、本车 `FleetMemberAgent` 和 `IVehicleStateProvider`；本车ID等于主车ID时，还必须传入 `FleetPreparationCoordinator`、`FleetCoordinator` 和 `FleetSafetySupervisor`。 +- `StartRollingPlan(planId, layout, trajectory)` 只允许主车调用；成员准备β直接取自 `FleetCoordinator.MotionDirectionInFleetRadians`，避免准备方向与控制方向出现两份配置。 +- `Update(currentTimeSeconds, deltaTimeSeconds)` 使用本机单调时间驱动准备超时、主车报告新鲜度和本地命令看门狗。主车缓存的远端报告采样时间必须已经换算到主车时间轴，当前运行层不会自行估计跨机时钟偏移。 +- `Stop(reason)` 正常取消本车任务；主车同时广播当前任务停止。运行状态覆盖 `Idle`、`Preparing`、`Ready`、`Active`、`Completed` 和锁存的 `Faulted`。 +- 第一版公开启动入口只支持固定布局滚动轨迹；协议和从车执行器虽已保留 `PrepareSpin`，但整队自转尚无对应主车任务入口。 + ## 轨迹契约 文件：`MultiWheelC/Trajectory/`。 diff --git a/docs/problems.md b/docs/problems.md index 0b336d7..48f0547 100644 --- a/docs/problems.md +++ b/docs/problems.md @@ -109,6 +109,7 @@ - 代码已经记录控制周期分段耗时、请求/限速后GCP命令和四舵角；M层已有轮速/舵角诊断CSV。 - 差速转舵角速度前馈默认增益0.9，曲率预瞄默认0.15s/0.12m。 - 2026-08-19第六轮共9131个轨迹控制周期：周期中位数约31.24ms、P95约32.68ms、最大约59.44ms；控制计算总耗时中位数约0.12ms、P95约0.22ms。当前C层计算不是主要周期瓶颈，历史约110ms现象不应继续归因于控制算法计算量。 +- 2026-08-25静止诊断确认：目标角和角速度前馈均为零时，右后轮仍可形成约1s量级的持续差速转舵往复；死区由0.1°增至0.5°后，另外三轮均停止输出，但一次自转模式切回正常模式后的右后轮仍在约-1.9°至+2.9°间振荡。约2°的瞬态偏差本身不能证明硬件故障，待定位对象是仅该轮不收敛的闭环动态差异；应先用降低公共比例增益的重复切换实验区分控制稳定裕量，再对四轮分别辨识延迟、增益、摩擦和方向不对称，明显离群轮组先排查编码器、机械间隙和低速驱动响应。 - 不同速度、载荷下的舵轮物理响应和前馈参数仍需按具体工况验证。 - CAN/MCU正常运行逻辑风险较高；除诊断外不应在没有明确方案和实车回退措施时修改。 diff --git a/docs/progress.md b/docs/progress.md index b697734..6ffb9b8 100644 --- a/docs/progress.md +++ b/docs/progress.md @@ -1,6 +1,6 @@ # 当前进展 -更新日期：2026-08-24。这里只保存当前状态，不作为完整开发历史。 +更新日期：2026-08-25。这里只保存当前状态，不作为完整开发历史。 ## 已完成/已接入 @@ -21,7 +21,8 @@ - 停车控制参数集中到 `Configuration/PilotConfig.ParkingControl.cs`。 - `PathTrackingContext` 已解除对单车 `VehicleState` 的依赖；`PathTrackingCore` 统一单车与车队的投影、保护/终点策略、横纵向控制和GCP分配，`ParkingGeometricController` 保留单车状态读取与实体命令执行职责。 - `MultiWheelC.Tests` 已提供不依赖实车的Stanley前进/倒车横向符号回归，8个场景通过；该项目不进入正式解决方案和打包脚本。 -- `Shared/Fleet` 已加入不可变 `FleetLayout`、成员/车队命令模型和确定性 `FleetKinematics`；`MultiWheelC/Fleet` 已具备 `FleetLayoutCapture`、`FleetStateEstimator`、固定 `β_fleet` 的 `FleetController`、`FleetMemberCommandCorrector`、`FleetCoordinator`、`FleetPreparationCoordinator` 和 `FleetMemberAgent`。这些组件覆盖布局采集、车队中心/成员误差估计、中心轨迹闭环、统一速度缩放、刚体分解、小范围成员纠偏、成员β准备屏障和本车命令执行边界，但尚未由正式车队任务运行入口贯通。 +- `Shared/Fleet` 已加入不可变 `FleetLayout`、成员/车队命令模型、确定性 `FleetKinematics` 及最小 `FleetCommand`/`FleetMemberReport` 契约；`MultiWheelC/Fleet` 已具备布局采集、车队状态估计、固定 `β_fleet` 中心控制、统一速度缩放、刚体分解、小范围成员纠偏、成员β准备屏障和本车执行边界。 +- 车队安全、通信边界和首版运行链已经贯通：`FleetRuntime` 按显式本车/主车ID执行主从分支，串联β准备屏障、全员激活、状态缓存、主车安全判定、周期协调、本车直接执行、远端分发、完成/取消和从车本地命令看门狗；`IFleetTransport` 隔离具体传输，测试项目提供不回环主车命令的内存实现。Fleet相关自动化测试共70个场景通过。 - 本次建立工作区/项目AGENTS导航和 `docs/` 按需知识库。 以上表示代码入口存在，不表示全部实车工况已经验收。 @@ -32,15 +33,16 @@ - 暂时冻结普通运动跳变阈值和30°/s、40°/s²自转参数，等待Detour接口语义后再决定是否实施自转结束后的条件化仅位置连续化或调整航向恢复策略。 - Detour对接最小问题已整理到 `docs/detour-information-checklist.md`，不要求取得源码。 - 验证非零β运动系，当前已有45°蟹行直线入口；曲线蟹行仍需设计实验。 -- 多车共同搬运的核心算法和本车执行组件已基本具备，当前重点不再是增加孤立算法文件，而是建立完整车队任务运行链：布局激活→β准备→全员Ready→统一激活→周期状态估计与协调→安全门控→成员命令分发→完成/故障/取消。没有这层调用者时，`FleetCoordinator` 的零速输出或任何独立安全判定都不会自动让实车停车。QP/HQP不作为第一版前置条件。 +- 多车共同搬运的固定布局滚动任务已在内存传输下完成端到端贯通。当前重点转为真实无线链路和工程接入：确认无线参数与帧格式，实现串口 `IFleetTransport`，接入公共坐标系成员状态和主车时间轴，再通过静止状态机与低速双车逐级验证。QP/HQP不作为第一版前置条件。 +- 车队状态估计第一版保留“成员状态先经单车状态估计处理、车队层做时间对齐与刚体一致性检查、候选中心等权/圆周平均”的保守方案；暂不重复实现单车Detour跳变逻辑，也不在运行链和实车数据建立前加入复杂鲁棒优化。后续内部融合升级应尽量保持 `FleetStateEstimateResult`、`FleetState` 和 `MemberErrors` 外部接口不变。 - β在多车中定位为单车执行坐标系而非车队核心优化量：常规、斜行和横移动作先确定车队主要滚动方向，各车按布局朝向换算本地β，停车预对齐并经车队同步屏障统一释放。轨迹级β搜索只作为机械余量或复杂方向变化下的后续增强。 ## 阻塞/待确认 - Clumsy/Medulla正式宿主、插件部署和配置持久化说明未纳入仓库。 - 完整停车业务流程和验收指标未确认。 -- 多车通信协议、统一时间轴、成员状态实际采集方式、布局原子激活接口、刚体误差实车阈值、夹紧/报警状态来源和故障降级策略未确认；车队参考点和纯几何采集方法已经确定，运行布局应在夹紧后生成不可变快照并由上层整体激活。 -- 实际部署为每车独立电脑。主车需要成员状态接收和命令分发，从车必须具备本地命令超时停车；仅依赖主车广播停止不能覆盖通信中断场景。 +- 最小内存消息契约已经确定，但真实无线串口的端口参数、帧格式、序列化/校验和跨机时钟换算尚未实现；成员状态实际采集方式、布局原子激活接口、刚体误差实车阈值、夹紧/报警状态来源和故障降级策略仍待确认。 +- 各车经过独立 `_controlFromDetour` 任务坐标连续化后的位姿如何统一到同一公共车队坐标系仍待验证。车队参考点和纯几何采集方法已经确定，运行布局应在夹紧后生成不可变快照并由上层整体激活。 - 最新实车实验数据对控制周期与舵轮滞后的结论尚未沉淀为可复核结果。 - Detour `l_step` 精确定义、显式重定位/坐标重置状态和部署端MDCS恢复策略待确认；当前不能把 `l_step<4` 当作唯一有效性条件。 - Detour `getCartLocation()` 的位姿坐标系、`tick` 采样/解算/发布语义、是否已有独立连续里程计或定位状态接口，以及部署版本和实际里程计/SLAM配置待确认。 @@ -53,7 +55,8 @@ 2. 在Detour信息返回前不继续全局放宽状态估计边界，也不提高自转角速度/角加速度；现有偶发定位不可用保持安全停车。 3. 为轮组里程计自转CSV补充请求角度、内部积分角、完成/超时/手动停止原因，按固定目标角重复验证误差和重复性；绝对航向任务继续保留Detour模式，不用轮组积分冒充世界航向。 4. 为轨迹插值/投影、坐标变换、状态跳变候选和终点策略继续补充不依赖宿主的数学回归测试。 -5. 先建立车队任务运行入口，把布局原子激活、`FleetPreparationCoordinator`、`FleetCoordinator` 和成员执行结果串成明确的生命周期；不要再增加没有运行消费者的独立安全判定器。 -6. 定义最小通信数据契约：成员状态、成员准备/执行状态、任务号、主车命令、心跳和命令有效期。无线串口初始化可后接，但运行层不能假设主车能直接调用另一台电脑上的 `FleetMemberAgent`。 -7. 接入成员状态实际来源和主车接收时间轴，形成 `FleetMemberStateSample[]`；再把 `FleetCoordinator` 输出按车号分发到各车 `FleetMemberAgent.Execute()`。 -8. 在真实执行链内实现安全门控：可恢复状态下持续下发零速并保留任务；硬故障时锁存任务失败、整队停止；每辆从车独立实现命令超时停车。随后补充无通信模拟的端到端测试，再进行低速双车实车验证。 +5. 确认无线模块的Windows COM端口、波特率、数据位、停止位、校验方式、收发模式和模块配置；定义带版本、长度、类型、任务/序列号及CRC的最小字节帧。 +6. 实现串口版 `IFleetTransport`，先只做主车心跳/命令和从车状态报告的静态通信测试，测量丢包、乱序、延迟和断线；不得绕过现有命令有效期与本车看门狗。 +7. 接入C层正式车队动作入口、成员状态实际来源和主车接收时间轴，保证 `FleetMemberReport` 位姿处于同一公共世界坐标系，远端采样时间已换算为主车时间后再形成 `FleetMemberStateSample[]`。 +8. 先验证静止布局建立、β准备、全员Ready、统一激活和停止，再进行空载低速双车直线与圆弧；共同搬运前补齐夹紧/报警输入和可立即停车措施。 +9. 记录各成员候选中心、数据年龄、状态来源、布局误差和减速/停车原因。只有数据证明当前等权融合频繁被单成员异常拖累时，再增加车队预测、逐成员门控和鲁棒加权。 diff --git a/output/C/CommonUsage.dll b/output/C/CommonUsage.dll index 2c0496a..d284e2e 100644 Binary files a/output/C/CommonUsage.dll and b/output/C/CommonUsage.dll differ diff --git a/output/C/MultiWheelC.dll b/output/C/MultiWheelC.dll index 59c03a9..666bccd 100644 Binary files a/output/C/MultiWheelC.dll and b/output/C/MultiWheelC.dll differ diff --git a/output/C/MultiWheelC.pdb b/output/C/MultiWheelC.pdb index 0106fc9..d62166c 100644 Binary files a/output/C/MultiWheelC.pdb and b/output/C/MultiWheelC.pdb differ diff --git a/output/M/CommonUsage.dll b/output/M/CommonUsage.dll index 2c0496a..d284e2e 100644 Binary files a/output/M/CommonUsage.dll and b/output/M/CommonUsage.dll differ diff --git a/output/M/MedullaAdapter.dll b/output/M/MedullaAdapter.dll index 13f09f7..6a3cd4b 100644 Binary files a/output/M/MedullaAdapter.dll and b/output/M/MedullaAdapter.dll differ diff --git a/output/M/MedullaAdapter.pdb b/output/M/MedullaAdapter.pdb index 1f8a518..dc4c9c6 100644 Binary files a/output/M/MedullaAdapter.pdb and b/output/M/MedullaAdapter.pdb differ diff --git a/ref/CommonUsage.dll b/ref/CommonUsage.dll index 2c0496a..d284e2e 100644 Binary files a/ref/CommonUsage.dll and b/ref/CommonUsage.dll differ diff --git a/参考文档/多车.txt b/参考文档/多车.txt new file mode 100644 index 0000000..70c48c3 --- /dev/null +++ b/参考文档/多车.txt @@ -0,0 +1,271 @@ +结论：`MultiWheelC\Fleet` 当前的数学链路作为“第一版组件”是成立的，现有 Fleet 数学测试共47个场景全部通过，没有发现明显的坐标系或符号错误。但它还不是可以直接上双车实车的完整工程方案，`FleetStateEstimator` 的等权平均尤其需要增加鲁棒性。 + +## 正负平均为0是不是问题 + +不一定，很多时候这正是期望结果。 + +假设两辆车反算出的车队中心分别为： + +```text +车辆1候选中心：+20mm +车辆2候选中心：-20mm +``` + +平均得到： + +```text +车队中心：0mm +``` + +但代码随后还会计算每辆车相对于这个中心的布局误差： + +```text +车辆1相对误差：+20mm +车辆2相对误差：-20mm +``` + +所以并不是误差消失了，而是被分成了： + +```text +公共分量：车队整体中心运动 +相对分量：成员之间的队形变形 +``` + +这正好符合当前架构： + +- 公共分量交给 `FleetController` 控制车队中心； +- 相对分量交给 `FleetCoordinator` 减速，以及 `FleetMemberCommandCorrector` 小幅纠偏。 + +如果两辆车反算结果都是 `+20mm`： + +```text +平均中心：+20mm +成员相对误差：接近0 +``` + +这表示整个车队共同移动了20mm，而队形没有变，也符合物理意义。 + +## 当前不是“无条件直接平均” + +当前 [FleetStateEstimator.cs](/D:/Users/Desktop/入职培训/停车机器人/MyParking/MultiWheelC/Fleet/FleetStateEstimator.cs:277) 实际执行的是： + +1. 将成员状态对齐到统一时刻。 +2. 每辆车根据固定布局反算候选车队中心。 +3. 检查任意两辆车反算的中心位置差和航向差。 +4. 差异超过阈值，直接返回状态不可用。 +5. 只有所有候选结果基本一致，才平均： + - X/Y算术平均； + - Yaw圆周平均。 +6. 根据融合后的中心重新计算每辆车的布局误差。 + +所以如果： + +```text +车辆1：+200mm +车辆2：-200mm +``` + +两者相差400mm，只要超过 `_maximumPositionDisagreementMeters`，代码会在平均前拒绝本周期状态，不会把它们平均成0后继续运行。 + +Yaw也不是普通平均。例如： + +```text ++179°和-179° +``` + +圆周平均会得到接近 `±180°`，而不是错误地得到 `0°`。 + +## 当前平均真正的问题 + +它隐含了一个假设： + +> 所有成员定位精度相同、可信度相同，而且没有异常值。 + +对你目前的 Detour 情况，这个假设不够可靠。 + +假如真实车队中心是0，而： + +```text +车辆1定位正常：0mm +车辆2发生小范围跳变：+60mm +``` + +当前等权平均会得到： + +```text +估计中心：+30mm +``` + +如果60mm还没有超过候选差异阈值，这个错误会进入车队中心控制。 + +当前估计器存在三个主要限制： + +- 所有成员等权，没有利用定位质量、数据年龄和历史稳定性； +- 单个异常成员超过阈值时，会使整个车队状态不可用，不能隔离坏成员； +- 它是单帧估计，没有利用上一周期车队中心和轮速预测判断哪辆车更可信。 + +而且你目前主要是双车。两辆车互相矛盾时不存在“多数票”： + +```text +车1说中心在A +车2说中心在B +``` + +只看这两个当前观测，无法判断谁正确。必须引入： + +- 上一周期车队状态； +- 轮速运动预测； +- 每辆车定位健康状态； +- 数据新鲜度； +- 连续多帧确认。 + +## 更成熟、适合你的方案 + +我建议采用： + +> 带时间预测和异常门控的鲁棒加权 SE(2) 融合。 + +不需要现在就上复杂的因子图或完整EKF。 + +### 第一步：保留当前反算方法 + +继续让每辆车计算： + +```text +候选车队位姿 += 成员实际世界位姿 +× 成员固定布局位姿的逆 +``` + +这部分当前是正确的，不需要推翻。 + +### 第二步：增加上一状态预测 + +根据上一周期车队中心和轮组速度预测： + +```text +PredictedFleetPose +``` + +然后每辆车的候选中心分别与预测值比较： + +```text +位置创新 +航向创新 +``` + +这样双车出现分歧时，可以判断： + +```text +车1与预测连续 +车2突然跳变 +→ 暂时拒绝车2，而不是两车平均或全队立即失败 +``` + +### 第三步：成员单独门控 + +每个成员分别检查： + +- 状态是否可用； +- 数据是否过期； +- 相对预测的位置创新是否合理； +- 相对预测的航向创新是否合理； +- 是否连续多帧异常； +- 后续是否连续多帧恢复。 + +不要只做当前这种“成员之间两两比较”。 + +### 第四步：对通过门控的候选加权融合 + +权重可以先用简单工程等级： + +```text +健康且新鲜 → 权重1.0 +轻度退化或较旧 → 较低权重 +正在异常确认 → 权重0 +``` + +以后 Detour 如果能提供协方差，再改成按协方差加权。 + +对残差使用 Huber 一类鲁棒损失会让正常小误差仍按最小二乘处理，而异常大残差的影响被压低；这是成熟估计库采用的标准做法，[GTSAM官方文档](https://gtsam.org/doxygen/a04439.html)也明确给出了 Huber 在迭代重加权最小二乘中的权重形式。 + +### 第五步：输出融合健康等级 + +建议以后区分： + +```text +Healthy +所有有效成员一致，正常融合 + +Degraded +有一个成员被拒绝，暂时依赖剩余成员和预测，整队限速 + +Unavailable +所有成员都不可信，或双车分歧且无法判断谁正确 +短时保持轮速预测，随后停车 +``` + +## 更严格的数学形式 + +成熟版本可以直接求一个最符合全部刚体观测的车队位姿： + +\[ +T_f^*= +\arg\min_{T_f} +\left[ +w_p\|\log(T_{pred}^{-1}T_f)\|^2+ +\sum_i w_i\,\rho\left( +\|\log((T_fT_{layout,i})^{-1}T_{actual,i})\|^2 +\right) +\right] +\] + +它表示： + +- `T_f`：要求解的车队实际中心； +- `T_layout,i`：第i辆车的固定布局； +- `T_actual,i`：第i辆车当前测量位姿； +- `T_pred`：根据历史和轮速得到的预测中心； +- `w_i`：成员可靠性； +- `ρ`：Huber等鲁棒损失。 + +这是“鲁棒刚体拟合”，比单纯平均更完整，同时利用位置和航向。普通最小二乘刚体配准本身就是成熟方法；鲁棒损失是在异常观测下限制单个数据影响的标准扩展。[刚体点集最小二乘原始问题说明](https://ntrl.ntis.gov/NTRL/dashboard/searchResults/titleDetail/PB91135772.xhtml) + +如果将来各车能提供可信协方差，但不同SLAM估计之间的相关性未知，可以考虑 Covariance Intersection；它专门处理未知交叉相关性的估计融合。[Julier与Uhlmann的SLAM融合论文](https://www.sciencedirect.com/science/article/abs/pii/S0921889006001436) 但你目前拿不到Detour协方差，所以现在直接上CI没有足够输入。 + +## 对当前项目的建议 + +当前先不要推翻 `FleetStateEstimator`。合理的推进顺序是： + +1. 保留现有布局反算、时间对齐、圆周Yaw平均和成员误差计算。 +2. 增加“上一车队状态＋轮速”的中心预测。 +3. 把两两一致性检查改成“每成员相对预测的独立门控”。 +4. 对通过门控的成员做加权圆周平均或小规模Huber融合。 +5. 允许单个成员短时降级，不要一帧异常就让整个估计不可用。 +6. 双车无法判断谁正确时，短时间用预测，仍无法恢复再停车。 +7. 最后再考虑完整的SE(2)鲁棒最小二乘，不需要一开始引入GTSAM或QP。 + +对整个 `MultiWheelC\Fleet` 的当前评价是： + +- 布局定义、刚体速度分解、中心控制、相对误差纠偏、最差成员统一减速和全员Ready屏障，结构上没有明显问题； +- 当前最大算法缺口就是车队中心融合还不够鲁棒； +- 最大工程缺口仍是运行入口、通信、成员超时看门狗、报警汇总和零速命令的实际下发； +- `FleetMemberAgent` 目前还缺少独立自动化测试； +- 各种阈值目前只在测试构造中出现，尚未通过双车实车数据标定。 + +所以，平均本身不是错误；“只做等权平均，并在冲突时整队不可用”才是需要下一阶段改进的部分。 + + + + + + +将 FleetMemberReport 转成 FleetMemberStateSample 和 FleetMemberSafetyStatus； +将准备命令映射到 FleetMemberAgent； +将成员速度命令映射成 FleetCommand； +ShouldStop 时广播 Stop 并让主车本地停车； +命令序号、报告序号和任务编号检查； +M层报警和底盘执行失败映射到 FailureCode； +无线端口初始化； +报文序列化、分包、校验和接收循环； +主车本地单调时钟和报文接收时间记录。 \ No newline at end of file diff --git a/参考文档/辨识.txt b/参考文档/辨识.txt new file mode 100644 index 0000000..48fa3de --- /dev/null +++ b/参考文档/辨识.txt @@ -0,0 +1,129 @@ +对，当前日志足够做分层建模，但物理因果顺序要明确： + +```text +目标舵角 TargetTh + ↓ +舵角PID/前馈控制器 + ↓ +左右电机实际下发命令 SentLeft / SentRight + ↓ +左右电机实际速度 ActualLeft / ActualRight + ↓ +左右轮差速产生舵轮转动 + ↓ +实际舵角 ActualTh +``` + +你可以建立以下几层模型。 + +1. 单个电机响应模型 + +分别辨识： + +```text +SentLeft → ActualLeft +SentRight → ActualRight +``` + +例如左前舵轮： + +```matlab +G_LFL = ActualLFL / SentLFLMps; +G_LFR = ActualLFR / SentLFRMps; +``` + +这可以得到左右电机各自的延迟、时间常数和增益，判断两个电机是否同步。 + +2. 差速轮对模型 + +计算： + +```matlab +uDiff = (SentRightMps - SentLeftMps) / 2; +vDiff = (ActualRight - ActualLeft) / 2; +``` + +辨识： + +```text +uDiff → vDiff +``` + +它表示控制器给出的差速命令，到两个电机真正形成差速运动的过程。 + +3. 差速运动到舵角模型 + +辨识： + +```text +vDiff → ActualTh +``` + +理论运动学关系是： + +\[ +\dot{\theta}=\frac{v_R-v_L}{d} +=\frac{2v_{\mathrm{diff}}}{d} +\] + +所以这一层理论上主要是积分环节： + +\[ +\theta(s)=\frac{2}{d\,s}v_{\mathrm{diff}}(s) +\] + +实际数据中还会包含摩擦、轮胎滑动、机械间隙和舵角反馈延迟。 + +4. 整体舵轮对象模型 + +直接辨识： + +```text +uDiff → ActualTh +``` + +这一模型包含： + +- 两个电机的响应延迟 +- 左右电机不同步 +- 差速转舵运动学 +- 摩擦和机械滞后 +- 舵角传感器延迟 + +这是后续设计舵角PID最有用的模型。 + +5. 完整闭环模型 + +还可以辨识： + +```text +TargetTh → ActualTh +``` + +它包含当前PID参数，适合评价现有系统的上升时间、超调和稳定时间，但不适合单独用来重新设计PID对象。 + +因此建议在Simulink中建立： + +```text +TargetTh + ↓ +Controller + ↓ +Motor Pair Dynamics + ↓ +Differential Steering Mechanics + ↓ +ActualTh + └────────反馈────────┘ +``` + +第一版控制率设计优先使用： + +```text +输入：uDiff +输出：ActualTh +``` + +单电机模型用于解释左右不同步和优化补偿。 + +需要注意：模式切换时主要激励的是差速通道，`uCommon≈0`。所以现有三组实验足够辨识转舵过程，但不能完整辨识车轮公共滚动速度对舵角的干扰。以后如果要研究车辆自转运行中的舵角漂移，还需补一次低速公共轮速实验。 \ No newline at end of file