@page "/motion/navigation-monitor" @rendermode InteractiveWebAssemblyNoPrerender @implements IAsyncDisposable @using RobotNet10.RobotApp.Client.Clients @using RobotNet10.RobotApp.Shared.NavigationMonitor @using Microsoft.AspNetCore.SignalR.Client @using MudBlazor @using ApexCharts @using Color = MudBlazor.Color @using Size = MudBlazor.Size Navigation Monitor
@* Connection status *@ @if (!hubClient.IsConnected) { Disconnected from Navigation Monitor hub. Reconnecting... } @* Controls *@ @if (telemetryEnabled) { @($"{updateFrequencyHz:F0} Hz") } else { Disabled } @* Safety Stop Latch Banner *@ @if (safetyStopLatched) { SAFETY STOP ACTIVE — @safetyStopReason Release Safety Stop } @* DockTo Telemetry - special panel when docking *@ @if (telemetry.DockTo is not null) { Docking Telemetry @telemetry.DockTo.Phase @telemetry.DockTo.Direction @{ var dock = telemetry.DockTo!; var svgBounds = GetDockSvgBounds(dock, telemetry.X, telemetry.Y); var vb = svgBounds; } @* SVG Visualization - left side *@ Docking Path
@* Grid reference lines *@ @* Waypoints path *@ @if (dock.Waypoints.Count >= 2) { var pathPoints = string.Join(" ", dock.Waypoints.Select(w => $"{w.X.ToString("F3")},{FlipY(w.Y).ToString("F3")}")); } @* Distance to goal line *@ @* Start Node *@ Start @* Goal Node with direction arrow *@ @{ var goalSvgY = FlipY(dock.GoalY); var arrowLen = vb.Width * 0.06; var goalArrowX = dock.GoalX + arrowLen * Math.Cos(dock.GoalTheta); var goalArrowY = goalSvgY - arrowLen * Math.Sin(dock.GoalTheta); } Goal @* Robot current position (triangle pointing in Theta direction) *@ @{ var rSvgY = FlipY(telemetry.Y); var rSize = vb.Width * 0.03; var rTheta = telemetry.Theta; // Triangle vertices: tip forward, two rear corners var tipX = telemetry.X + rSize * 1.5 * Math.Cos(rTheta); var tipY = rSvgY - rSize * 1.5 * Math.Sin(rTheta); var leftX = telemetry.X + rSize * Math.Cos(rTheta + 2.5); var leftY = rSvgY - rSize * Math.Sin(rTheta + 2.5); var rightX = telemetry.X + rSize * Math.Cos(rTheta - 2.5); var rightY = rSvgY - rSize * Math.Sin(rTheta - 2.5); } Robot @* Arrow marker definition *@
@* Docking Info - right side *@ Phase @telemetry.DockTo.Phase Direction@telemetry.DockTo.Direction Fine Positioning Retries @telemetry.DockTo.RetryCount / @telemetry.DockTo.MaxRetries Waypoints@telemetry.DockTo.TotalWaypoints Goal X@telemetry.DockTo.GoalX.ToString("F3") m Goal Y@telemetry.DockTo.GoalY.ToString("F3") m Goal Theta@((telemetry.DockTo.GoalTheta * 180 / Math.PI).ToString("F2"))° Distance to Goal @telemetry.DistanceToGoal.ToString("F3") m } @* Telemetry Data Cards *@ @* Position *@ Position X@telemetry.X.ToString("F3") m Y@telemetry.Y.ToString("F3") m Theta@((telemetry.Theta * 180 / Math.PI).ToString("F2"))° @* Velocity *@ Velocity Linear@telemetry.LinearVelocity.ToString("F3") m/s Angular@telemetry.AngularVelocity.ToString("F3") rad/s Confidence@telemetry.ModelConfidence.ToString("F2") @* Navigation State *@ Navigation State @telemetry.NavigationState Driving@(telemetry.Driving ? "Yes" : "No") To Goal@telemetry.DistanceToGoal.ToString("F3") m @* Tracking (CTE) *@ Tracking Cross-Track Error @telemetry.CrossTrackError.ToString("F3") m Heading Error @((telemetry.HeadingError * 180 / Math.PI).ToString("F2"))° @* Acceleration *@ Acceleration Linear @telemetry.LinearAcceleration.ToString("F2") m/s² Angular @telemetry.AngularAcceleration.ToString("F2") m/s² @* Telemetry Charts *@ Telemetry Charts Cross-Track Error Velocity Heading Error @* Navigation Path Visualization *@ @if (_cachedWaypoints is { Count: >= 2 }) { Navigation Path @_cachedWaypoints.Count waypoints @telemetry.NavigationState @{ var wp = _cachedWaypoints; var pvb = GetPathSvgBounds(wp, _robotTrail); var pStroke = (pvb.Width * 0.005).ToString("F4"); var pThin = (pvb.Width * 0.003).ToString("F4"); var pDash = (pvb.Width * 0.01).ToString("F4"); }
@* Grid reference lines *@ @* Waypoints path line *@ @{ var pathLine = string.Join(" ", wp.Select(w => $"{w.X.ToString("F3")},{FlipY(w.Y).ToString("F3")}")); } @* Waypoint dots *@ @for (int wi = 0; wi < wp.Count; wi++) { var wpt = wp[wi]; var wColor = wpt.Direction == "BACKWARD" ? "#ff9800" : "#5b9bd5"; var wRadius = (pvb.Width * 0.008).ToString("F4"); } @* Start waypoint *@ @{ var wpStart = wp[0]; var startR = (pvb.Width * 0.02).ToString("F4"); } Start @* Goal waypoint *@ @{ var wpGoal = wp[^1]; var goalR = (pvb.Width * 0.02).ToString("F4"); } Goal @* Robot trail (breadcrumb) *@ @if (_robotTrail.Count >= 2) { var trailLine = string.Join(" ", _robotTrail.Select(t => $"{t.X.ToString("F3")},{FlipY(t.Y).ToString("F3")}")); } @* Legend *@ @{ var legendX = pvb.MinX + pvb.Width * 0.02; var legendY = pvb.MinY + pvb.Height * 0.06; var legendFs = (pvb.Width * 0.025).ToString("F4"); var legendR = (pvb.Width * 0.008).ToString("F4"); var legendStep = pvb.Height * 0.05; } Forward Backward Robot Trail
} @* Safety Configuration *@
Safety Configuration Apply
@* Safety Violations *@ Safety Violations Clear @if (violations.Count == 0) { No violations recorded. } else {
@foreach (var v in violations) { @DateTimeOffset.FromUnixTimeMilliseconds(v.TimestampMs).ToLocalTime().ToString("HH:mm:ss.fff") — @v.Message }
}
@code { [Inject] private NavigationMonitorHubClient hubClient { get; set; } = null!; [Inject] private ISnackbar Snackbar { get; set; } = null!; private NavigationTelemetryDto telemetry = new(); private NavigationSafetyConfigDto safetyConfig = new(); private List violations = new(); private bool telemetryEnabled; private bool safetyStopEnabled; private bool safetyStopLatched; private string safetyStopReason = ""; private double updateFrequencyHz = 10; private const int MaxViolations = 100; // UI render throttle: render at 2Hz private const int UiRefreshIntervalMs = 500; private System.Threading.Timer? _uiRefreshTimer; private volatile bool _telemetryDirty; // Chart data & ApexCharts private List chartDataPoints = new(); private const int MaxChartPoints = 120; private long chartStartTimeMs; private volatile bool _chartDirty; // Path visualization: cached waypoints persist after navigation ends private List _cachedWaypoints = new(); private List<(double X, double Y)> _robotTrail = new(); private const int MaxTrailPoints = 120; private ApexChart? _cteChart; private ApexChart? _velocityChart; private ApexChart? _headingChart; private ApexChartOptions _cteOptions = new(); private ApexChartOptions _velocityOptions = new(); private ApexChartOptions _headingOptions = new(); protected override async Task OnAfterRenderAsync(bool firstRender) { if (firstRender) { ConfigureChartOptions(); hubClient.TelemetryReceived += OnTelemetryReceived; hubClient.SafetyViolationReceived += OnSafetyViolationReceived; hubClient.MonitorStateChanged += OnMonitorStateChanged; hubClient.ConnectionStateChanged += OnConnectionStateChanged; _uiRefreshTimer = new System.Threading.Timer(OnUiRefreshTick, null, UiRefreshIntervalMs, UiRefreshIntervalMs); try { await hubClient.StartAsync(); var state = await hubClient.GetStateAsync(); ApplyMonitorState(state); StateHasChanged(); } catch (Exception ex) { Snackbar.Add($"Failed to connect: {ex.Message}", Severity.Error); } } } private void OnTelemetryReceived(NavigationTelemetryDto dto) { telemetry = dto; // Cache waypoints: update when new path arrives, persist after navigation ends if (dto.Waypoints is { Count: >= 2 }) { // New waypoints = new navigation task → reset trail if (!WaypointsMatch(_cachedWaypoints, dto.Waypoints)) { _cachedWaypoints = dto.Waypoints; _robotTrail.Clear(); } } // Track robot trail (keep trail after navigation ends for visualization) if (dto.Driving) { _robotTrail.Add((dto.X, dto.Y)); if (_robotTrail.Count > MaxTrailPoints) _robotTrail.RemoveAt(0); } // Add chart data point every telemetry tick (now 2Hz from server) if (chartStartTimeMs == 0) chartStartTimeMs = dto.TimestampMs; chartDataPoints.Add(new TelemetryPoint { TimeSec = (dto.TimestampMs - chartStartTimeMs) / 1000.0, Cte = dto.CrossTrackError, LinearVel = dto.LinearVelocity, AngularVel = dto.AngularVelocity, HeadingErrorDeg = dto.HeadingError * 180 / Math.PI }); if (chartDataPoints.Count > MaxChartPoints) chartDataPoints.RemoveRange(0, chartDataPoints.Count - MaxChartPoints); _telemetryDirty = true; _chartDirty = true; } private void OnUiRefreshTick(object? state) { if (!_telemetryDirty) return; _telemetryDirty = false; InvokeAsync(async () => { if (_chartDirty) { _chartDirty = false; try { if (_cteChart is not null) await _cteChart.UpdateSeriesAsync(true); if (_velocityChart is not null) await _velocityChart.UpdateSeriesAsync(true); if (_headingChart is not null) await _headingChart.UpdateSeriesAsync(true); } catch (ObjectDisposedException) { } } StateHasChanged(); }); } private void OnSafetyViolationReceived(NavigationSafetyViolationDto dto) { violations.Insert(0, dto); if (violations.Count > MaxViolations) violations.RemoveRange(MaxViolations, violations.Count - MaxViolations); InvokeAsync(StateHasChanged); } private void OnMonitorStateChanged(NavigationMonitorStateDto state) { ApplyMonitorState(state); InvokeAsync(StateHasChanged); } private void OnConnectionStateChanged(HubConnectionState state) { InvokeAsync(StateHasChanged); } private void ApplyMonitorState(NavigationMonitorStateDto state) { telemetryEnabled = state.TelemetryEnabled; safetyStopEnabled = state.SafetyStopEnabled; safetyStopLatched = state.SafetyStopLatched; safetyStopReason = state.SafetyStopReason; updateFrequencyHz = state.UpdateFrequencyHz; safetyConfig = state.SafetyConfig; } private async Task OnTelemetryToggle(bool enabled) { try { telemetryEnabled = enabled; if (enabled) { // Reset chart history when re-enabling chartDataPoints.Clear(); chartStartTimeMs = 0; _cachedWaypoints.Clear(); _robotTrail.Clear(); } await hubClient.SetTelemetryEnabledAsync(enabled); } catch (Exception ex) { Snackbar.Add($"Error: {ex.Message}", Severity.Error); } } private async Task OnSafetyStopToggle(bool enabled) { try { safetyStopEnabled = enabled; await hubClient.SetSafetyStopEnabledAsync(enabled); } catch (Exception ex) { Snackbar.Add($"Error: {ex.Message}", Severity.Error); } } private async Task OnReleaseSafetyStop() { try { await hubClient.ReleaseSafetyStopAsync(); Snackbar.Add("Safety stop released", Severity.Info); } catch (Exception ex) { Snackbar.Add($"Error: {ex.Message}", Severity.Error); } } private async Task ApplySafetyConfig() { try { await hubClient.UpdateSafetyConfigAsync(safetyConfig); Snackbar.Add("Safety config updated", Severity.Success); } catch (Exception ex) { Snackbar.Add($"Error: {ex.Message}", Severity.Error); } } private void ClearViolations() { violations.Clear(); } private MudBlazor.Color GetNavStateColor() => telemetry.NavigationState switch { "Moving" or "Rotating" => MudBlazor.Color.Info, "Docking" or "FinePositioning" => MudBlazor.Color.Tertiary, "Completed" => MudBlazor.Color.Success, "Error" or "Canceled" or "SafetyStop" => MudBlazor.Color.Error, "Paused" => MudBlazor.Color.Warning, _ => MudBlazor.Color.Default }; private MudBlazor.Color GetDockPhaseColor() => telemetry.DockTo?.Phase switch { "Approaching" => MudBlazor.Color.Info, "Aligning" => MudBlazor.Color.Warning, "Advancing" => MudBlazor.Color.Success, _ => MudBlazor.Color.Default }; // SVG helpers for docking visualization private record struct SvgBounds(double MinX, double MinY, double Width, double Height); private SvgBounds GetDockSvgBounds(DockToTelemetryDto dock, double robotX, double robotY) { var xs = new List { dock.StartX, dock.GoalX, robotX }; var ys = new List { dock.StartY, dock.GoalY, robotY }; foreach (var w in dock.Waypoints) { xs.Add(w.X); ys.Add(w.Y); } double minX = xs.Min(), maxX = xs.Max(); double minY = ys.Min(), maxY = ys.Max(); // Ensure minimum size and add padding double rangeX = maxX - minX; double rangeY = maxY - minY; if (rangeX < 0.5) { minX -= 0.25; rangeX = 0.5; maxX = minX + rangeX; } if (rangeY < 0.5) { minY -= 0.25; rangeY = 0.5; maxY = minY + rangeY; } double pad = Math.Max(rangeX, rangeY) * 0.15; // Flip Y: SVG Y-down, world Y-up. We flip individual Y coords with FlipY(), // so viewBox uses flipped min/max double svgMinX = minX - pad; double svgMinY = -(maxY + pad); // flipped double svgW = rangeX + 2 * pad; double svgH = rangeY + 2 * pad; return new SvgBounds(svgMinX, svgMinY, svgW, svgH); } private static string FormatViewBox(SvgBounds vb) => $"{vb.MinX.ToString("F3")} {vb.MinY.ToString("F3")} {vb.Width.ToString("F3")} {vb.Height.ToString("F3")}"; private static double FlipY(double worldY) => -worldY; // SVG helpers for navigation path visualization private static SvgBounds GetPathSvgBounds(List waypoints, List<(double X, double Y)> trail) { var xs = waypoints.Select(w => w.X).ToList(); var ys = waypoints.Select(w => w.Y).ToList(); foreach (var t in trail) { xs.Add(t.X); ys.Add(t.Y); } double minX = xs.Min(), maxX = xs.Max(); double minY = ys.Min(), maxY = ys.Max(); double rangeX = maxX - minX; double rangeY = maxY - minY; if (rangeX < 0.5) { minX -= 0.25; rangeX = 0.5; maxX = minX + rangeX; } if (rangeY < 0.5) { minY -= 0.25; rangeY = 0.5; maxY = minY + rangeY; } double pad = Math.Max(rangeX, rangeY) * 0.15; double svgMinX = minX - pad; double svgMinY = -(maxY + pad); double svgW = rangeX + 2 * pad; double svgH = rangeY + 2 * pad; return new SvgBounds(svgMinX, svgMinY, svgW, svgH); } /// /// Quick check if waypoints are the same path (compare first/last + count) /// private static bool WaypointsMatch(List a, List b) { if (a.Count != b.Count || a.Count == 0) return false; return Math.Abs(a[0].X - b[0].X) < 0.001 && Math.Abs(a[0].Y - b[0].Y) < 0.001 && Math.Abs(a[^1].X - b[^1].X) < 0.001 && Math.Abs(a[^1].Y - b[^1].Y) < 0.001; } // ApexCharts configuration (following TelemetryChartPanel pattern) private void ConfigureChartOptions() { var axisColor = "#009933"; var baseGrid = () => new Grid { BorderColor = "#00993330", StrokeDashArray = 4, Xaxis = new GridXAxis { Lines = new Lines { Show = false } }, Yaxis = new GridYAxis { Lines = new Lines { Show = true } } }; var baseChart = () => new Chart { ForeColor = axisColor, Animations = new Animations { Enabled = false }, Toolbar = new Toolbar { Show = false }, Zoom = new Zoom { Enabled = false } }; var baseXAxis = () => new XAxis { Title = new AxisTitle { Text = "Time (s)", Style = new AxisTitleStyle { Color = axisColor } }, Labels = new XAxisLabels { Formatter = @"function(val) { return parseFloat(val).toFixed(0); }", Style = new AxisLabelStyle { Colors = axisColor } } }; var baseTooltip = () => new Tooltip { Shared = true, X = new TooltipX { Format = "0.1f" }, Y = new TooltipY { Formatter = @"function(val) { return val.toFixed(4); }" } }; var yAxisStyle = new AxisTitleStyle { Color = axisColor }; var yLabelStyle = new AxisLabelStyle { Colors = axisColor }; // CTE chart — blue _cteOptions.Chart = baseChart(); _cteOptions.Grid = baseGrid(); _cteOptions.Stroke = new Stroke { Curve = Curve.Smooth, Width = 3 }; _cteOptions.Xaxis = baseXAxis(); _cteOptions.Colors = new List { "#0D47A1" }; _cteOptions.Tooltip = baseTooltip(); _cteOptions.Yaxis = new List { new YAxis { Title = new AxisTitle { Text = "CTE (m)", Style = yAxisStyle }, Labels = new YAxisLabels { Formatter = @"function(val) { return val.toFixed(3); }", Style = yLabelStyle }, DecimalsInFloat = 3, Min = 0 } }; // Velocity chart — green (linear) + purple (angular) _velocityOptions.Chart = baseChart(); _velocityOptions.Grid = baseGrid(); _velocityOptions.Stroke = new Stroke { Curve = Curve.Smooth, Width = 3 }; _velocityOptions.Xaxis = baseXAxis(); _velocityOptions.Colors = new List { "#1B5E20", "#7B1FA2" }; _velocityOptions.Tooltip = baseTooltip(); _velocityOptions.Yaxis = new List { new YAxis { Title = new AxisTitle { Text = "Velocity", Style = yAxisStyle }, Labels = new YAxisLabels { Formatter = @"function(val) { return val.toFixed(3); }", Style = yLabelStyle }, DecimalsInFloat = 3 } }; // Heading error chart — orange _headingOptions.Chart = baseChart(); _headingOptions.Grid = baseGrid(); _headingOptions.Stroke = new Stroke { Curve = Curve.Smooth, Width = 3 }; _headingOptions.Xaxis = baseXAxis(); _headingOptions.Colors = new List { "#E65100" }; _headingOptions.Tooltip = baseTooltip(); _headingOptions.Yaxis = new List { new YAxis { Title = new AxisTitle { Text = "Heading (deg)", Style = yAxisStyle }, Labels = new YAxisLabels { Formatter = @"function(val) { return val.toFixed(2); }", Style = yLabelStyle }, DecimalsInFloat = 2 } }; } private class TelemetryPoint { public double TimeSec { get; set; } public double Cte { get; set; } public double LinearVel { get; set; } public double AngularVel { get; set; } public double HeadingErrorDeg { get; set; } } public async ValueTask DisposeAsync() { if (_uiRefreshTimer is not null) await _uiRefreshTimer.DisposeAsync(); hubClient.TelemetryReceived -= OnTelemetryReceived; hubClient.SafetyViolationReceived -= OnSafetyViolationReceived; hubClient.MonitorStateChanged -= OnMonitorStateChanged; hubClient.ConnectionStateChanged -= OnConnectionStateChanged; await hubClient.DisposeAsync(); } }