feat: host EM observation dashboard
This commit is contained in:
梁薄云
+91
-13
@@ -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;
|
||||
|
||||
+145
@@ -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<string> 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<string> 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<PlanningVisualizationDynamicSnapshot> 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();
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user