diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs index 4c089e8..142bb77 100644 --- a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/MovementTest.TrajectoryObservationTest.cs @@ -15,6 +15,7 @@ using MultiWheelC.TrajectoryPlanning.CoarsePath; using MultiWheelC.TrajectoryPlanning.CoarsePath.Facade; using MultiWheelC.TrajectoryPlanning.EMPlanner; using MultiWheelC.TrajectoryPlanning.TrajectoryObservation; +using TrajectoryPlanningVisualization; namespace MultiWheelC; @@ -196,11 +197,11 @@ internal static class TrajectoryObservationMovementTestRunner private const string StatusChannel = "TrajectoryObserver"; private const string ObserveOnlyNotice = "OBSERVE_ONLY: no chassis command is sent."; private static readonly object SessionSync = new object(); - private static readonly TrajectoryObservationPresentation Presentation = - new TrajectoryObservationPresentation(); private static CancellationTokenSource activeCancellation; private static Task activeTask; + private static TrajectoryObservationVisualizationPublisher activeWebPublisher; + private static TrajectoryObservationPresentation activePresentation; private static long nextSessionId; private static long activeSessionId; private static long stateSequence; @@ -222,7 +223,8 @@ internal static class TrajectoryObservationMovementTestRunner activeTask = null; sessionId = ++nextSessionId; activeSessionId = sessionId; - Presentation.ClearAll(); + StopActiveVisualizationNoLock(); + activePresentation = settings.EnableNativePainterVisualization ? new TrajectoryObservationPresentation() : null; PrintStatus("Starting frozen observation session " + sessionId.ToString(CultureInfo.InvariantCulture) + "."); } @@ -247,9 +249,7 @@ internal static class TrajectoryObservationMovementTestRunner activeCancellation = null; activeTask = null; activeSessionId = 0L; - if (TrajectoryObservationSessionLifecycle.ShouldClearLayers( - TrajectoryObservationSessionEndReason.Cancellation)) - Presentation.ClearAll(); + StopActiveVisualizationNoLock(); PrintStatus("Observation stop requested; all observer layers were cleared."); } @@ -293,7 +293,10 @@ internal static class TrajectoryObservationMovementTestRunner "trajectory-observer-" + sessionId.ToString(CultureInfo.InvariantCulture)); LogIfCurrent(sessionId, controller.CreateConfigurationDiagnostic().Text); var observationLoop = new TrajectoryObservationLoop(controller); - DirectionSegmentView segment = bootstrap.Segments[0]; + TrajectoryObservationVisualizationPublisher webPublisher = StartWebVisualizationIfEnabled( + sessionId, bootstrap, controller, settings, obstacleSnapshotVersion); + var dynamicSnapshotBuilder = new TrajectoryObservationDynamicSnapshotBuilder(); + long visualizationSequence = 0L; bool gearSwitchWaitingPrinted = false; while (true) @@ -306,6 +309,7 @@ internal static class TrajectoryObservationMovementTestRunner TrajectoryObservationDiagnostic diagnostic = TrajectoryObservationDiagnostics.Create( tick.LatestCycle, tick.LatestPlanningElapsed, tick.PlanningInFlight, observation.PublishedTrajectory); + DirectionSegmentView segment = controller.ActiveSegment; TrajectoryObservationCharts charts = observation.PublishedTrajectory == null ? null : TrajectoryObservationCharts.Build(observation.PublishedTrajectory, segment, @@ -313,6 +317,9 @@ internal static class TrajectoryObservationMovementTestRunner TrajectoryObservationRuntimeState runtimeState = TrajectoryObservationRuntimeState.Create( now, observation.PublishedTrajectory); DrawIfCurrent(sessionId, bootstrap, observation, charts, runtimeState, diagnostic); + webPublisher?.TryPublish(now, () => dynamicSnapshotBuilder.Build( + Interlocked.Increment(ref visualizationSequence), tick, controller.ActiveSegment, + controller.PreviousTrajectoryForVisualization, controller.CreateEffectiveConfigurationSnapshot())); if (runtimeState.WaitingAtGearSwitch && !gearSwitchWaitingPrinted) { LogIfCurrent(sessionId, runtimeState.WorldNotice); @@ -395,6 +402,66 @@ internal static class TrajectoryObservationMovementTestRunner "LS projection failures=" + projectionFailures + "." + waiting; } + private static TrajectoryObservationVisualizationPublisher StartWebVisualizationIfEnabled(long sessionId, + TrajectoryObservationBootstrapResult bootstrap, TrajectoryObservationController controller, + TrajectoryObservationSettings settings, long obstacleSnapshotVersion) + { + if (!settings.EnableWebVisualization) return null; + + var publisher = new TrajectoryObservationVisualizationPublisher(settings, + new PlanningVisualizationSessionSink(), message => LogIfCurrent(sessionId, message)); + PlanningVisualizationSessionInfo sessionInfo; + try + { + var staticSnapshot = new TrajectoryObservationStaticSnapshotBuilder().Build(bootstrap, + controller.CreateEffectiveConfigurationSnapshot(), settings, obstacleSnapshotVersion); + sessionInfo = publisher.Start(staticSnapshot); + } + catch (Exception exception) + { + publisher.Disable(exception); + return null; + } + + if (sessionInfo == null) + return null; + if (!TryAttachWebPublisher(sessionId, publisher)) + { + publisher.Stop(); + return null; + } + + LogIfCurrent(sessionId, "网页可视化地址（含会话令牌）：" + sessionInfo.Uri.AbsoluteUri); + if (settings.AutoOpenWebVisualization) + { + try + { + Process.Start(new ProcessStartInfo + { + FileName = sessionInfo.Uri.AbsoluteUri, + UseShellExecute = true, + }); + } + catch (Exception exception) + { + LogIfCurrent(sessionId, "无法自动打开网页；服务继续运行，请手动访问：" + + sessionInfo.Uri.AbsoluteUri + "；" + exception.GetType().Name + "：" + exception.Message); + } + } + return publisher; + } + + private static bool TryAttachWebPublisher(long sessionId, + TrajectoryObservationVisualizationPublisher publisher) + { + lock (SessionSync) + { + if (activeSessionId != sessionId) return false; + activeWebPublisher = publisher; + return true; + } + } + private static void DrawIfCurrent(long sessionId, TrajectoryObservationBootstrapResult bootstrap, TrajectoryObservationObservation observation, TrajectoryObservationCharts charts, TrajectoryObservationRuntimeState runtimeState, TrajectoryObservationDiagnostic diagnostic) @@ -402,10 +469,12 @@ internal static class TrajectoryObservationMovementTestRunner lock (SessionSync) { if (activeSessionId != sessionId) return; + TrajectoryObservationPresentation presentation = activePresentation; + if (presentation == null) return; string diagnosticText = diagnostic == null ? string.Empty : diagnostic.Text; - Presentation.DrawWorld(bootstrap, observation, runtimeState, diagnosticText); - Presentation.DrawLs(charts, diagnosticText); - Presentation.DrawSt(charts, diagnosticText); + presentation.DrawWorld(bootstrap, observation, runtimeState, diagnosticText); + presentation.DrawLs(charts, diagnosticText); + presentation.DrawSt(charts, diagnosticText); } } @@ -457,14 +526,23 @@ internal static class TrajectoryObservationMovementTestRunner lock (SessionSync) { if (activeSessionId != sessionId) return; - if (TrajectoryObservationSessionLifecycle.ShouldClearLayers( - TrajectoryObservationSessionEndReason.RuntimeFault)) - Presentation.ClearAll(); + StopActiveVisualizationNoLock(); PrintStatus("Observation session failed; all observer layers were cleared: " + exception.GetType().Name + ": " + exception.Message); } } + private static void StopActiveVisualizationNoLock() + { + TrajectoryObservationVisualizationPublisher publisher = activeWebPublisher; + activeWebPublisher = null; + publisher?.Stop(); + + TrajectoryObservationPresentation presentation = activePresentation; + activePresentation = null; + presentation?.ClearAll(); + } + private static void CancelWithoutWaiting(CancellationTokenSource cancellation) { if (cancellation == null) return; diff --git a/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationVisualizationPublisher.cs b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationVisualizationPublisher.cs new file mode 100644 index 0000000..f8ed8eb --- /dev/null +++ b/ClumsyPilot/ParkrobTrajplanner/tarjplanner_movementtest/TrajectoryObservationVisualizationPublisher.cs @@ -0,0 +1,145 @@ +using System; +using System.Runtime.CompilerServices; +using TrajectoryPlanningVisualization; + +[assembly: InternalsVisibleTo("EMPlannerVerificationHost")] + +namespace MultiWheelC.TrajectoryPlanning.TrajectoryObservation; + +internal interface ITrajectoryObservationVisualizationSink +{ + PlanningVisualizationSessionInfo Start(PlanningVisualizationOptions options, + PlanningVisualizationStaticSnapshot snapshot); + void Publish(PlanningVisualizationDynamicSnapshot snapshot); + void Stop(); +} + +internal sealed class TrajectoryObservationVisualizationPublisher +{ + private readonly TrajectoryObservationSettings settings; + private readonly ITrajectoryObservationVisualizationSink sink; + private readonly Action log; + private DateTimeOffset nextPublishAtUtc; + private bool started; + private bool stopped; + private bool sinkStopped; + private string faultReason = string.Empty; + + internal TrajectoryObservationVisualizationPublisher(TrajectoryObservationSettings settings, + ITrajectoryObservationVisualizationSink sink, Action log) + { + this.settings = settings ?? throw new ArgumentNullException(nameof(settings)); + this.sink = sink ?? throw new ArgumentNullException(nameof(sink)); + this.log = log ?? throw new ArgumentNullException(nameof(log)); + } + + internal string FaultReason => faultReason; + + internal PlanningVisualizationSessionInfo Start(PlanningVisualizationStaticSnapshot snapshot) + { + if (!settings.EnableWebVisualization || stopped || faultReason.Length != 0) + return null; + if (started) return null; + + try + { + if (snapshot == null) throw new ArgumentNullException(nameof(snapshot)); + PlanningVisualizationSessionInfo session = sink.Start(new PlanningVisualizationOptions + { + Port = settings.WebVisualizationPort, + RefreshRateHz = Math.Max(1, (int)Math.Round(settings.WebRefreshRateHz, + MidpointRounding.AwayFromZero)), + HistoryCycleLimit = settings.VisualizationHistoryCycleLimit, + }, snapshot); + if (session == null) throw new InvalidOperationException("网页会话未返回访问地址。"); + started = true; + return session; + } + catch (Exception exception) + { + Fuse(exception); + return null; + } + } + + internal bool TryPublish(DateTimeOffset now, Func snapshotFactory) + { + if (snapshotFactory == null) throw new ArgumentNullException(nameof(snapshotFactory)); + if (!settings.EnableWebVisualization || !started || stopped || faultReason.Length != 0) + return false; + if (now < nextPublishAtUtc) return false; + + try + { + PlanningVisualizationDynamicSnapshot snapshot = snapshotFactory(); + if (snapshot == null) throw new InvalidOperationException("网页动态快照为空。"); + nextPublishAtUtc = now + TimeSpan.FromSeconds(1d / settings.WebRefreshRateHz); + sink.Publish(snapshot); + return true; + } + catch (Exception exception) + { + Fuse(exception); + return false; + } + } + + internal void Disable(Exception exception) + { + if (exception == null) throw new ArgumentNullException(nameof(exception)); + Fuse(exception); + } + + internal void Stop() + { + if (stopped) return; + stopped = true; + StopSink(); + } + + private void Fuse(Exception exception) + { + if (faultReason.Length != 0) return; + faultReason = "网页可视化已关闭：" + exception.GetType().Name + "：" + exception.Message; + StopSink(); + log(faultReason); + } + + private void StopSink() + { + if (sinkStopped) return; + sinkStopped = true; + try + { + sink.Stop(); + } + catch + { + } + } +} + +internal sealed class PlanningVisualizationSessionSink : ITrajectoryObservationVisualizationSink +{ + private PlanningVisualizationSession session; + + public PlanningVisualizationSessionInfo Start(PlanningVisualizationOptions options, + PlanningVisualizationStaticSnapshot snapshot) + { + if (session != null) return session.Start(snapshot); + session = new PlanningVisualizationSession(options); + return session.Start(snapshot); + } + + public void Publish(PlanningVisualizationDynamicSnapshot snapshot) + { + session?.Publish(snapshot); + } + + public void Stop() + { + PlanningVisualizationSession current = session; + session = null; + current?.Dispose(); + } +} diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs index 6a24b48..6e33504 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationChecks.cs @@ -24,6 +24,7 @@ internal static class TrajectoryObservationChecks VerifiesObservationSourceUsesRequiredOperatorText(); VerifiesOperatorDocumentationUsesExactUiEntry(); VerifiesMovementTestVehicleInputsAndSettingsSnapshot(); + VerifiesMovementTestKeepsNativePainterLazyAndConditional(); VerifiesPlanningConfigurationDiagnosticUsesEffectiveConfiguration(); RejectsInvalidObservationSettings(); VerifiesStartGoalBoundsUseOnlyConfiguredPadding(); @@ -1025,6 +1026,17 @@ internal static class TrajectoryObservationChecks } } + private static void VerifiesMovementTestKeepsNativePainterLazyAndConditional() + { + string movementTestPath = Path.Combine(Directory.GetCurrentDirectory(), "ClumsyPilot", + "ParkrobTrajplanner", "tarjplanner_movementtest", "MovementTest.TrajectoryObservationTest.cs"); + string source = new UTF8Encoding(false, true).GetString(File.ReadAllBytes(movementTestPath)); + Verification.True(!source.Contains("static readonly TrajectoryObservationPresentation"), + "MovementTest has no static eager Painter presentation"); + Verification.True(source.Contains("settings.EnableNativePainterVisualization ? new TrajectoryObservationPresentation() : null"), + "MovementTest creates Painter only under the native visualization flag"); + } + private static bool Throws(Action action) { try { action(); return false; } diff --git a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationVisualizationChecks.cs b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationVisualizationChecks.cs index 060c0af..f6f85e4 100644 --- a/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationVisualizationChecks.cs +++ b/ClumsyPilot/tests/EMPlannerVerificationHost/TrajectoryObservationVisualizationChecks.cs @@ -1,6 +1,7 @@ using System; using System.Collections.Generic; using System.Globalization; +using System.IO; using System.Linq; using System.Reflection; using System.Threading; @@ -20,6 +21,8 @@ internal static class TrajectoryObservationVisualizationChecks VerifiesKinematicChartsKeepKnotAndIntervalSemantics(); VerifiesHandoffUsesSharedReferenceSAndSurvivesProjectionFailure(); VerifiesDynamicSnapshotContainsObservationEvidenceWithoutCyclePoints(); + VerifiesWebPublisherFusesFaultWithoutStoppingObserverTicks(); + VerifiesWebPublisherGatesAtConfiguredCadenceAndSkipsDisabledOutput(); } private static void VerifiesStaticSnapshotExportsFrozenConfigurationAndGeometry() @@ -215,6 +218,116 @@ internal static class TrajectoryObservationVisualizationChecks "projection failure is reported while dynamic snapshot construction remains available"); } + private static void VerifiesWebPublisherFusesFaultWithoutStoppingObserverTicks() + { + var settings = new TrajectoryObservationSettings { EnableWebVisualization = true }; + var sink = new RecordingVisualizationSink { ThrowOnPublish = true }; + var logs = new List(); + var publisher = new TrajectoryObservationVisualizationPublisher(settings, sink, logs.Add); + DateTimeOffset now = DateTimeOffset.UnixEpoch; + + publisher.Start(CreateStaticSnapshot(settings)); + Verification.True(!publisher.TryPublish(now, () => CreateDynamicSnapshot(now)), + "publisher reports a failed web publish"); + Verification.True(!publisher.TryPublish(now.AddSeconds(1d), () => throw new InvalidOperationException("must not build after fuse")), + "publisher permanently disables web output after the first fault"); + Verification.Equal(1, sink.PublishCalls, "publisher invokes the throwing sink once"); + Verification.Equal(1, sink.StopCalls, "publisher stops the sink once after its fault"); + Verification.Equal(1, logs.Count, "publisher logs the web fault once"); + Verification.True(publisher.FaultReason.Contains("网页"), "publisher records an immutable Chinese web fault reason"); + + TrajectoryObservationBootstrapResult bootstrap = Bootstrap(settings); + DateTimeOffset tickAt = now.AddSeconds(1d); + var controller = new TrajectoryObservationController(bootstrap, settings, + new FixedPlanningService(CreateTrajectory("publisher-loop", tickAt, 2, EmTerminalType.Goal, + EmLongitudinalMode.RollingContinuation, TravelDirection.Forward)), "publisher-loop"); + var loop = new TrajectoryObservationLoop(controller); + var state = new VehicleMotionState(new Pose2D(0d, 0d, 0d), 0d, null, tickAt, 1L); + TrajectoryObservationLoopTick firstTick = loop.Tick(tickAt, state, CancellationToken.None); + TrajectoryObservationLoopTick secondTick = loop.Tick(tickAt.AddSeconds(settings.ObserverPeriodSeconds), state, + CancellationToken.None); + Verification.True(firstTick.PlanningStarted || secondTick.PlanningStarted || secondTick.PlanningCompleted, + "observer loop continues producing ticks after a web publisher fault"); + } + + private static void VerifiesWebPublisherGatesAtConfiguredCadenceAndSkipsDisabledOutput() + { + var settings = new TrajectoryObservationSettings + { + EnableWebVisualization = true, + WebRefreshRateHz = 10d, + }; + var sink = new RecordingVisualizationSink(); + var publisher = new TrajectoryObservationVisualizationPublisher(settings, sink, _ => { }); + DateTimeOffset start = DateTimeOffset.UnixEpoch; + publisher.Start(CreateStaticSnapshot(settings)); + for (int index = 0; index <= 20; index++) + publisher.TryPublish(start.AddMilliseconds(index * 50), () => CreateDynamicSnapshot(start)); + Verification.True(sink.PublishCalls <= 11, "20 Hz observer ticks publish no more than 11 snapshots at 10 Hz over one second"); + Verification.Equal(11, sink.PublishCalls, "publisher includes the initial 10 Hz snapshot"); + publisher.Stop(); + publisher.Stop(); + Verification.Equal(1, sink.StopCalls, "publisher stop is idempotent"); + + var disabledSettings = new TrajectoryObservationSettings { EnableWebVisualization = false }; + var disabledSink = new RecordingVisualizationSink(); + var disabledPublisher = new TrajectoryObservationVisualizationPublisher(disabledSettings, disabledSink, _ => { }); + int factoryCalls = 0; + disabledPublisher.Start(CreateStaticSnapshot(disabledSettings)); + disabledPublisher.TryPublish(start, () => + { + factoryCalls++; + return CreateDynamicSnapshot(start); + }); + Verification.Equal(0, disabledSink.StartCalls, "disabled web visualization does not start a service"); + Verification.Equal(0, factoryCalls, "disabled web visualization does not build dynamic snapshots"); + } + + private static PlanningVisualizationStaticSnapshot CreateStaticSnapshot(TrajectoryObservationSettings settings) + { + TrajectoryObservationBootstrapResult bootstrap = Bootstrap(settings); + var controller = new TrajectoryObservationController(bootstrap, settings, + new FixedPlanningService(CreateTrajectory("publisher-static", DateTimeOffset.UnixEpoch, 2, + EmTerminalType.Goal, EmLongitudinalMode.RollingContinuation, TravelDirection.Forward)), "publisher-static"); + return new TrajectoryObservationStaticSnapshotBuilder().Build(bootstrap, + ReadEffectiveConfiguration(controller), settings.CreateValidatedSnapshot(), 0L); + } + + private static PlanningVisualizationDynamicSnapshot CreateDynamicSnapshot(DateTimeOffset now) + { + return new PlanningVisualizationDynamicSnapshot(1L, now, "OBSERVE_ONLY", 0, "Forward", + new VisualizationPose(0d, 0d, 0d), Array.Empty(), + Array.Empty(), Array.Empty(), Array.Empty(), + new VisualizationCycleSummary(1L, now, "Success", true, 0d, 0, "Forward", "RollingContinuation", + "Goal", 0d, 0d, string.Empty)); + } + + private sealed class RecordingVisualizationSink : ITrajectoryObservationVisualizationSink + { + public bool ThrowOnPublish { get; set; } + public int StartCalls { get; private set; } + public int PublishCalls { get; private set; } + public int StopCalls { get; private set; } + + public PlanningVisualizationSessionInfo Start(PlanningVisualizationOptions options, + PlanningVisualizationStaticSnapshot snapshot) + { + StartCalls++; + return new PlanningVisualizationSessionInfo(new Uri("http://127.0.0.1:12345/?token=test"), "test"); + } + + public void Publish(PlanningVisualizationDynamicSnapshot snapshot) + { + PublishCalls++; + if (ThrowOnPublish) throw new InvalidOperationException("synthetic publish failure"); + } + + public void Stop() + { + StopCalls++; + } + } + private static TrajectoryObservationBootstrapResult Bootstrap(TrajectoryObservationSettings settings) { CoarsePathPlanningJob job = TrajectoryObservationSetupFactory.CreateBootstrapJob(