diff --git a/RobotApp.Client/MainLayout.razor b/RobotApp.Client/MainLayout.razor index 5af009e..b066067 100644 --- a/RobotApp.Client/MainLayout.razor +++ b/RobotApp.Client/MainLayout.razor @@ -71,6 +71,7 @@ public NavModel[] Navs = [ new(){Icon = "mdi-view-dashboard", Path="/", Label = "Dashboard", Match = NavLinkMatch.All}, // new(){Icon = "mdi-map-legend", Path="/maps-manager", Label = "Mapping", Match = NavLinkMatch.All}, + new(){Icon = "mdi-monitor", Path="/robot-monitor", Label = "Robot Monitor", Match = NavLinkMatch.All}, new(){Icon = "mdi-application-cog", Path="/robot-config", Label = "Config", Match = NavLinkMatch.All}, new(){Icon = "mdi-application-cog", Path="/robot-state", Label = "state", Match = NavLinkMatch.All}, ]; diff --git a/RobotApp.Client/Pages/Components/Monitor/RobotMonitorView.razor b/RobotApp.Client/Pages/Components/Monitor/RobotMonitorView.razor new file mode 100644 index 0000000..497d67a --- /dev/null +++ b/RobotApp.Client/Pages/Components/Monitor/RobotMonitorView.razor @@ -0,0 +1,342 @@ +@inject IJSRuntime JS + +
+
+ + + + + + + + + + + @if (MonitorData?.RobotPosition != null) + { +
+ + X: @MonitorData.RobotPosition.X.ToString("F2")m | Y: @MonitorData.RobotPosition.Y.ToString("F2")m | θ: @((MonitorData.RobotPosition.Theta * 180 / Math.PI).ToString("F1"))° +
+ } + + @(IsConnected ? "Connected" : "Disconnected") + +
+
+ + + @if (MonitorData?.HasOrder == true) + { + @* @for (int i = 0; i < MonitorData.EdgeStates.Length; i++) + { + var edge = MonitorData.EdgeStates[i]; + var (startX, startY, endX, endY) = GetEdgeEndpoints(i, edge); + + } *@ + + + @foreach (var node in MonitorData.NodeStates) + { + + } + } + + @* Render Robot *@ + @if (MonitorData?.RobotPosition != null) + { + + @{ + var (robotX, robotY, robotWidth, robotHeight) = GetRobotSize(); + } + + + } + + +
+
+ +@code { + [Parameter] public RobotMonitorDto? MonitorData { get; set; } + [Parameter] public bool IsConnected { get; set; } + + public class ElementSize + { + public double Width { get; set; } + public double Height { get; set; } + } + + public class ElementBoundingRect + { + public double X { get; set; } + public double Y { get; set; } + public double Width { get; set; } + public double Height { get; set; } + } + + private ElementReference SvgRef; + private ElementReference SvgContainerRef; + + private double ZoomScale = 2.0; // Zoom vào robot hơn khi mở + private const double MIN_ZOOM = 0.1; + private const double MAX_ZOOM = 5.0; + private const double BASE_PIXELS_PER_METER = 50.0; + + private double TranslateX = 0; + private double TranslateY = 0; + + private bool IsPanning = false; + private double PanStartX = 0; + private double PanStartY = 0; + + private double SvgWidth = 800; + private double SvgHeight = 600; + + private string PathView = ""; + private string PathIsNot = "hidden"; + + protected override async Task OnAfterRenderAsync(bool firstRender) + { + if (firstRender) + { + var containerSize = await JS.InvokeAsync("getElementSize", SvgContainerRef); + SvgWidth = containerSize.Width; + SvgHeight = containerSize.Height; + + // Center view on robot if available with initial zoom + if (MonitorData?.RobotPosition != null) + { + // Zoom vào robot hơn một chút + ZoomScale = 2.5; + TranslateX = SvgWidth / 2 - MonitorData.RobotPosition.X * BASE_PIXELS_PER_METER * ZoomScale; + TranslateY = SvgHeight / 2 + MonitorData.RobotPosition.Y * BASE_PIXELS_PER_METER * ZoomScale; + } + else + { + TranslateX = SvgWidth / 2; + TranslateY = SvgHeight / 2; + } + + StateHasChanged(); + } + } + + private string GetTransform() + { + return $"translate({TranslateX}, {TranslateY}) scale({ZoomScale * BASE_PIXELS_PER_METER})"; + } + + private string GetRobotTransform() + { + if (MonitorData?.RobotPosition == null) return ""; + var x = WorldToSvgX(MonitorData.RobotPosition.X); + var y = WorldToSvgY(MonitorData.RobotPosition.Y); + // Theta là radian, convert sang độ + // SVG rotate quay theo chiều kim đồng hồ, cần đảo dấu + var angleDegrees = -MonitorData.RobotPosition.Theta * 180 / Math.PI; + return $"translate({x}, {y}) rotate({angleDegrees})"; + } + + private (double x, double y, double width, double height) GetRobotSize() + { + // Kích thước robot trong world coordinates (mét) + const double RobotWidthMeters = 0.606; + const double RobotLengthMeters = 1.106; + + // Điều chỉnh kích thước dựa trên ZoomScale + // Tăng kích thước lên 1.5x để robot to hơn + double scaleFactor = 3 / ZoomScale; // Tăng kích thước hiển thị + + double width = RobotWidthMeters * scaleFactor; + double height = RobotLengthMeters * scaleFactor; + double x = -width / 2; + double y = -height / 2; + + return (x, y, width, height); + } + + private double GetNodeRadius() + { + // Kích thước node cơ bản trong world coordinates - tăng lên 1.5x + const double BaseNodeRadius = 0.15; + + // Điều chỉnh theo ZoomScale tương tự robot + double scaleFactor = 1.5 / ZoomScale; // Tăng kích thước hiển thị + + return BaseNodeRadius * scaleFactor; + } + + private double GetNodeStrokeWidth() + { + // Stroke width cơ bản - tăng lên một chút + const double BaseStrokeWidth = 0.03; + + // Điều chỉnh theo ZoomScale + double scaleFactor = 1.5 / ZoomScale; + + return BaseStrokeWidth * scaleFactor; + } + + private double WorldToSvgX(double worldX) + { + return worldX; + } + + private double WorldToSvgY(double worldY) + { + // Flip Y axis: World Y↑ → SVG Y↓ + return -worldY; + } + + public void UpdatePath() + { + if (MonitorData is not null && MonitorData.EdgeStates.Length > 0) + { + var path = MonitorData.EdgeStates.Select(e => new EdgeStateDto + { + Degree = e.Degree, + StartX = WorldToSvgX(e.StartX), + StartY = WorldToSvgY(e.StartY), + EndX = WorldToSvgX(e.EndX), + EndY = WorldToSvgY(e.EndY), + ControlPoint1X = WorldToSvgX(e.ControlPoint1X), + ControlPoint1Y = WorldToSvgY(e.ControlPoint1Y), + ControlPoint2X = WorldToSvgX(e.ControlPoint2X), + ControlPoint2Y = WorldToSvgY(e.ControlPoint2Y), + }).ToList(); + var inPath = $"M {path[0].StartX} {path[0].StartY}"; + for (int i = 0; i < path.Count; i++) + { + if (path[i].Degree == 1) inPath = $"{inPath} L {path[i].EndX} {path[i].EndY}"; + else if (path[i].Degree == 2) inPath = $"{inPath} Q {path[i].ControlPoint1X} {path[i].ControlPoint1Y} {path[i].EndX} {path[i].EndY}"; + else inPath = $"{inPath} C {path[i].ControlPoint1X} {path[i].ControlPoint1Y}, {path[i].ControlPoint2X} {path[i].ControlPoint2Y}, {path[i].EndX} {path[i].EndY}"; + } + PathView = inPath; + PathIsNot = "visible"; + } + else + { + PathView = ""; + PathIsNot = "hidden"; + } + StateHasChanged(); + } + + private async Task ZoomIn() + { + ZoomScale = Math.Min(MAX_ZOOM, ZoomScale * 1.15); + await Task.CompletedTask; + StateHasChanged(); + } + + private async Task ZoomOut() + { + ZoomScale = Math.Max(MIN_ZOOM, ZoomScale * 0.85); + await Task.CompletedTask; + StateHasChanged(); + } + + private async Task ResetView() + { + // Reset về zoom ban đầu (2.5x) khi có robot position + if (MonitorData?.RobotPosition != null) + { + ZoomScale = 2.5; + TranslateX = SvgWidth / 2 - MonitorData.RobotPosition.X * BASE_PIXELS_PER_METER * ZoomScale; + TranslateY = SvgHeight / 2 + MonitorData.RobotPosition.Y * BASE_PIXELS_PER_METER * ZoomScale; + } + else + { + ZoomScale = 1.0; + TranslateX = SvgWidth / 2; + TranslateY = SvgHeight / 2; + } + await Task.CompletedTask; + StateHasChanged(); + } + + private void HandleMouseDown(MouseEventArgs e) + { + IsPanning = true; + PanStartX = e.ClientX - TranslateX; + PanStartY = e.ClientY - TranslateY; + } + + private void HandleMouseMove(MouseEventArgs e) + { + if (IsPanning) + { + TranslateX = e.ClientX - PanStartX; + TranslateY = e.ClientY - PanStartY; + StateHasChanged(); + } + } + + private void HandleMouseUp(MouseEventArgs e) + { + IsPanning = false; + } + + private void HandleMouseLeave(MouseEventArgs e) + { + IsPanning = false; + } + + private async Task HandleWheel(WheelEventArgs e) + { + const double zoomFactor = 0.1; + double oldZoom = ZoomScale; + + if (e.DeltaY < 0) + ZoomScale = Math.Min(MAX_ZOOM, ZoomScale * (1 + zoomFactor)); + else + ZoomScale = Math.Max(MIN_ZOOM, ZoomScale * (1 - zoomFactor)); + + if (Math.Abs(ZoomScale - oldZoom) < 0.001) return; + + // Zoom at mouse position + var svgRect = await JS.InvokeAsync("getElementBoundingRect", SvgRef); + double mouseX = e.ClientX - svgRect.X; + double mouseY = e.ClientY - svgRect.Y; + + // Calculate world coordinates at mouse position + double worldX = (mouseX - TranslateX) / (oldZoom * BASE_PIXELS_PER_METER); + double worldY = -(mouseY - TranslateY) / (oldZoom * BASE_PIXELS_PER_METER); + + // Adjust translate to keep world point under mouse + TranslateX = mouseX - worldX * ZoomScale * BASE_PIXELS_PER_METER; + TranslateY = mouseY + worldY * ZoomScale * BASE_PIXELS_PER_METER; + + StateHasChanged(); + } +} + diff --git a/RobotApp.Client/Pages/Components/Monitor/RobotMonitorView.razor.css b/RobotApp.Client/Pages/Components/Monitor/RobotMonitorView.razor.css new file mode 100644 index 0000000..85f2893 --- /dev/null +++ b/RobotApp.Client/Pages/Components/Monitor/RobotMonitorView.razor.css @@ -0,0 +1,74 @@ +.robot-monitor-container { + display: flex; + flex-direction: column; + width: 100%; + height: 100%; + overflow: hidden; +} + +.toolbar { + display: flex; + align-items: center; + padding: 8px 16px; + background-color: #2d2d2d; + border-bottom: 1px solid #444; + gap: 8px; + min-height: 48px; +} + +.action-button { + padding: 8px; + border: none; + background: #3d3d3d; + border-radius: 4px; + cursor: pointer; + display: flex; + align-items: center; + justify-content: center; + color: #fff; + transition: background-color 0.2s; +} + +.action-button:hover { + background: #4d4d4d; +} + +.action-button:active { + background: #5d5d5d; +} + +.icon-button { + font-size: 20px; + color: #fff; +} + +.robot-position-info { + color: #fff; + font-size: 14px; + font-family: 'Segoe UI', Tahoma, Geneva, Verdana, sans-serif; + padding: 4px 12px; + background-color: #3d3d3d; + border-radius: 4px; + display: flex; + align-items: center; + gap: 8px; +} + +.svg-container { + flex: 1; + overflow: hidden; + position: relative; + background-color: #fafafa; +} + +.svg-container svg { + width: 100%; + height: 100%; + cursor: grab; + background-color: dimgray; +} + +.svg-container svg:active { + cursor: grabbing; +} + diff --git a/RobotApp.Client/Pages/RobotMonitor.razor b/RobotApp.Client/Pages/RobotMonitor.razor new file mode 100644 index 0000000..f871ece --- /dev/null +++ b/RobotApp.Client/Pages/RobotMonitor.razor @@ -0,0 +1,41 @@ +@page "/robot-monitor" +@rendermode InteractiveWebAssemblyNoPrerender +@attribute [Authorize] +@inject RobotApp.Client.Services.RobotMonitorService MonitorService +@implements IAsyncDisposable + +Robot Monitor + +
+ +
+ +@code { + private RobotMonitorDto? _monitorData; + private RobotApp.Client.Pages.Components.Monitor.RobotMonitorView? RobotMonitorViewRef; + + protected override async Task OnAfterRenderAsync(bool firstRender) + { + if (firstRender) + { + MonitorService.OnDataReceived += OnMonitorDataReceived; + await MonitorService.StartAsync(); + } + } + + private void OnMonitorDataReceived(RobotMonitorDto data) + { + _monitorData = data; + RobotMonitorViewRef?.UpdatePath(); + InvokeAsync(StateHasChanged); + } + + public async ValueTask DisposeAsync() + { + MonitorService.OnDataReceived -= OnMonitorDataReceived; + await MonitorService.StopAsync(); + } +} + diff --git a/RobotApp.Client/Program.cs b/RobotApp.Client/Program.cs index 6c76300..3dcc49a 100644 --- a/RobotApp.Client/Program.cs +++ b/RobotApp.Client/Program.cs @@ -10,6 +10,7 @@ builder.Services.AddCascadingAuthenticationState(); builder.Services.AddAuthenticationStateDeserialization(); builder.Services.AddScoped(_ => new HttpClient() { BaseAddress = new Uri(builder.HostEnvironment.BaseAddress) }); +builder.Services.AddScoped(); builder.Services.AddMudServices(config => { config.SnackbarConfiguration.VisibleStateDuration = 2000; diff --git a/RobotApp.Client/Services/RobotMonitorService.cs b/RobotApp.Client/Services/RobotMonitorService.cs new file mode 100644 index 0000000..d7660c1 --- /dev/null +++ b/RobotApp.Client/Services/RobotMonitorService.cs @@ -0,0 +1,52 @@ +using Microsoft.AspNetCore.Components; +using Microsoft.AspNetCore.SignalR.Client; +using RobotApp.Common.Shares.Dtos; + +namespace RobotApp.Client.Services; + +public class RobotMonitorService : IAsyncDisposable +{ + private HubConnection? _hubConnection; + private readonly string _hubUrl; + + public event Action? OnDataReceived; + public bool IsConnected => _hubConnection?.State == HubConnectionState.Connected; + + public RobotMonitorService(NavigationManager navigationManager) + { + var baseUrl = navigationManager.BaseUri.TrimEnd('/'); + _hubUrl = $"{baseUrl}/hubs/robotMonitor"; + } + + public async Task StartAsync() + { + if (_hubConnection is not null) return; + + _hubConnection = new HubConnectionBuilder() + .WithUrl(_hubUrl) + .WithAutomaticReconnect() + .Build(); + + _hubConnection.On("ReceiveRobotMonitorData", data => + { + OnDataReceived?.Invoke(data); + }); + + await _hubConnection.StartAsync(); + } + + public async Task StopAsync() + { + if (_hubConnection is null) return; + + await _hubConnection.StopAsync(); + await _hubConnection.DisposeAsync(); + _hubConnection = null; + } + + public async ValueTask DisposeAsync() + { + await StopAsync(); + } +} + diff --git a/RobotApp.Client/wwwroot/js/robotMonitor.js b/RobotApp.Client/wwwroot/js/robotMonitor.js new file mode 100644 index 0000000..4f2800f --- /dev/null +++ b/RobotApp.Client/wwwroot/js/robotMonitor.js @@ -0,0 +1,60 @@ +// Helper functions for Robot Monitor + +window.robotMonitor = { + // Get element size + getElementSize: function (element) { + const rect = element.getBoundingClientRect(); + return { + Width: rect.width, + Height: rect.height + }; + }, + + // Get element bounding rect + getElementBoundingRect: function (element) { + const rect = element.getBoundingClientRect(); + return { + X: rect.x, + Y: rect.y, + Width: rect.width, + Height: rect.height + }; + }, + + // Convert trajectory to SVG path + trajectoryToPath: function (trajectory, startX, startY, endX, endY) { + if (!trajectory || !trajectory.ControlPoints || trajectory.ControlPoints.length === 0) { + // Linear path + return `M ${startX} ${startY} L ${endX} ${endY}`; + } + + const degree = trajectory.Degree || 1; + const controlPoints = trajectory.ControlPoints; + + if (degree === 1) { + // Linear + return `M ${startX} ${startY} L ${endX} ${endY}`; + } else if (degree === 2) { + // Quadratic bezier + if (controlPoints.length > 0) { + const cp1 = controlPoints[0]; + return `M ${startX} ${startY} Q ${cp1.X} ${cp1.Y} ${endX} ${endY}`; + } + return `M ${startX} ${startY} L ${endX} ${endY}`; + } else if (degree === 3) { + // Cubic bezier + if (controlPoints.length >= 2) { + const cp1 = controlPoints[0]; + const cp2 = controlPoints[1]; + return `M ${startX} ${startY} C ${cp1.X} ${cp1.Y}, ${cp2.X} ${cp2.Y}, ${endX} ${endY}`; + } else if (controlPoints.length === 1) { + const cp1 = controlPoints[0]; + return `M ${startX} ${startY} Q ${cp1.X} ${cp1.Y} ${endX} ${endY}`; + } + return `M ${startX} ${startY} L ${endX} ${endY}`; + } + + return `M ${startX} ${startY} L ${endX} ${endY}`; + } +}; + diff --git a/RobotApp.Common.Shares/Dtos/RobotMonitorDto.cs b/RobotApp.Common.Shares/Dtos/RobotMonitorDto.cs new file mode 100644 index 0000000..8161da2 --- /dev/null +++ b/RobotApp.Common.Shares/Dtos/RobotMonitorDto.cs @@ -0,0 +1,31 @@ +using RobotApp.VDA5050.State; + +namespace RobotApp.Common.Shares.Dtos; + +public class RobotMonitorDto +{ + public RobotPositionDto RobotPosition { get; set; } = new(); + public EdgeStateDto[] EdgeStates { get; set; } = []; + public NodeState[] NodeStates { get; set; } = []; + public bool HasOrder { get; set; } + public DateTime Timestamp { get; set; } = DateTime.UtcNow; +} + +public class RobotPositionDto +{ + public double X { get; set; } + public double Y { get; set; } + public double Theta { get; set; } +} +public class EdgeStateDto() +{ + public double StartX { get; set; } + public double StartY { get; set; } + public double EndX { get; set; } + public double EndY { get; set; } + public double ControlPoint1X { get; set; } + public double ControlPoint1Y { get; set; } + public double ControlPoint2X { get; set; } + public double ControlPoint2Y { get; set; } + public int Degree { get; set; } +} diff --git a/RobotApp.Common.Shares/RobotApp.Common.Shares.csproj b/RobotApp.Common.Shares/RobotApp.Common.Shares.csproj index b760144..3419a7e 100644 --- a/RobotApp.Common.Shares/RobotApp.Common.Shares.csproj +++ b/RobotApp.Common.Shares/RobotApp.Common.Shares.csproj @@ -6,4 +6,8 @@ enable + + + + diff --git a/RobotApp/Hubs/RobotMonitorHub.cs b/RobotApp/Hubs/RobotMonitorHub.cs new file mode 100644 index 0000000..604ac59 --- /dev/null +++ b/RobotApp/Hubs/RobotMonitorHub.cs @@ -0,0 +1,12 @@ +using Microsoft.AspNetCore.SignalR; + +namespace RobotApp.Hubs; + +public class RobotMonitorHub : Hub +{ + public async Task SendRobotMonitorData(RobotApp.Common.Shares.Dtos.RobotMonitorDto data) + { + await Clients.All.SendAsync("ReceiveRobotMonitorData", data); + } +} + diff --git a/RobotApp/Interfaces/IOrder.cs b/RobotApp/Interfaces/IOrder.cs index 810b08e..cfe1c0b 100644 --- a/RobotApp/Interfaces/IOrder.cs +++ b/RobotApp/Interfaces/IOrder.cs @@ -1,4 +1,5 @@ -using RobotApp.VDA5050.Order; +using RobotApp.Common.Shares.Dtos; +using RobotApp.VDA5050.Order; using RobotApp.VDA5050.State; namespace RobotApp.Interfaces; @@ -12,6 +13,7 @@ public interface IOrder NodeState[] NodeStates { get; } EdgeState[] EdgeStates { get; } + (NodeState[], EdgeStateDto[]) CurrentPath { get; } void UpdateOrder(OrderMsg order); void StopOrder(); diff --git a/RobotApp/Program.cs b/RobotApp/Program.cs index a4938c5..ac55b4d 100644 --- a/RobotApp/Program.cs +++ b/RobotApp/Program.cs @@ -7,6 +7,7 @@ using RobotApp.Client; using RobotApp.Components; using RobotApp.Components.Account; using RobotApp.Data; +using RobotApp.Hubs; using RobotApp.Services; using RobotApp.Services.Robot.Simulation; @@ -50,9 +51,16 @@ builder.Services.AddSingleton, IdentityNoOpEmailSe builder.Services.AddSingleton(typeof(RobotApp.Services.Logger<>)); +// Add SignalR +builder.Services.AddSignalR(); + builder.Services.AddRobotSimulation(); builder.Services.AddRobot(); +// Add RobotMonitorService +builder.Services.AddSingleton(); +builder.Services.AddHostedService(sp => sp.GetRequiredService()); + var app = builder.Build(); await app.Services.SeedApplicationDbAsync(); @@ -81,6 +89,9 @@ app.MapStaticAssets(); // Map API Controllers app.MapControllers(); +// Map SignalR Hub +app.MapHub("/hubs/robotMonitor"); + app.MapRazorComponents() .AddInteractiveServerRenderMode() .AddInteractiveWebAssemblyRenderMode() diff --git a/RobotApp/Services/Robot/RobotControllerInitialize.cs b/RobotApp/Services/Robot/RobotControllerInitialize.cs index 4579dc8..022bc75 100644 --- a/RobotApp/Services/Robot/RobotControllerInitialize.cs +++ b/RobotApp/Services/Robot/RobotControllerInitialize.cs @@ -54,8 +54,6 @@ public partial class RobotController } } else StateManager.TransitionTo(RootStateType.Auto); - - ErrorManager.AddError(RobotErrors.Error2001()); } catch (Exception ex) { diff --git a/RobotApp/Services/Robot/RobotOrderController.cs b/RobotApp/Services/Robot/RobotOrderController.cs index 8111e36..730f17d 100644 --- a/RobotApp/Services/Robot/RobotOrderController.cs +++ b/RobotApp/Services/Robot/RobotOrderController.cs @@ -1,4 +1,5 @@ using MudBlazor; +using RobotApp.Common.Shares.Dtos; using RobotApp.Common.Shares.Enums; using RobotApp.Interfaces; using RobotApp.Services.Exceptions; @@ -27,6 +28,7 @@ public class RobotOrderController(INavigation NavigationManager, public EdgeState[] EdgeStates { get; private set; } = []; public string LastNodeId => LastNode is null ? "" : LastNode.NodeId; public int LastNodeSequenceId => LastNode is null ? 0 : LastNode.SequenceId; + public (NodeState[], EdgeStateDto[]) CurrentPath => GetCurrentPath(); private const int CycleHandlerMilliseconds = 100; private WatchTimer? OrderTimer; @@ -40,6 +42,8 @@ public class RobotOrderController(INavigation NavigationManager, private VDA5050.Order.Edge[] Edges = []; private Node? CurrentBaseNode; private Node? LastNode; + private Node[] NewOrderNodes = []; + private VDA5050.Order.Edge[] NewOrderEdges = []; private readonly Lock LockObject = new(); @@ -223,7 +227,7 @@ public class RobotOrderController(INavigation NavigationManager, SafetyManager.OnSafetySpeedChanged += OnSafetySpeedChanged; if (OrderActions.TryGetValue(order.Nodes[^1].NodeId, out Action[]? finalactions) && finalactions is not null && finalactions.Length > 0) FinalAction = [.. finalactions]; - + if (OrderActions.Count > 0) ActionManager.AddOrderActions([.. OrderActions.Values.SelectMany(a => a)]); if (order.Nodes.Length > 1 && order.Edges.Length >= 0) @@ -236,6 +240,8 @@ public class RobotOrderController(INavigation NavigationManager, OrderUpdateId = order.OrderUpdateId; Nodes = order.Nodes; Edges = order.Edges; + NewOrderNodes = order.Nodes; + NewOrderEdges = order.Edges; if (CurrentBaseNode is not null && CurrentBaseNode.NodeId != Nodes[0].NodeId && Nodes.Length > 1) NavigationManager.UpdateOrder(CurrentBaseNode.NodeId); if (StateManager.CurrentStateName != AutoStateType.Executing.ToString()) StateManager.TransitionTo(AutoStateType.Executing); UpdateState(); @@ -412,4 +418,68 @@ public class RobotOrderController(INavigation NavigationManager, } } + private EdgeStateDto[] SplitChecking(Node lastNode, Node nearLastNode, VDA5050.Order.Edge edge) + { + List pathEdges = []; + var splitStartPath = RobotPathPlanner.PathSplit([lastNode, nearLastNode], [edge], 0.1); + if (splitStartPath is not null && splitStartPath.Length > 0) + { + int index = 0; + double minDistance = double.MaxValue; + for (int i = 0; i < splitStartPath.Length; i++) + { + var distance = Math.Sqrt(Math.Pow(Localization.X - splitStartPath[i].NodePosition.X, 2) + + Math.Pow(Localization.Y - splitStartPath[i].NodePosition.Y, 2)); + if (distance < minDistance) + { + minDistance = distance; + index = i; + } + } + for (int i = index; i < splitStartPath.Length - 1; i++) + { + pathEdges.Add(new() + { + StartX = splitStartPath[i].NodePosition.X, + StartY = splitStartPath[i].NodePosition.Y, + EndX = splitStartPath[i + 1].NodePosition.X, + EndY = splitStartPath[i + 1].NodePosition.Y, + Degree = 1, + }); + } + } + return [.. pathEdges]; + } + + private (NodeState[], EdgeStateDto[]) GetCurrentPath() + { + if (NodeStates.Length == 0 && EdgeStates.Length == 0) return ([], []); + + List pathEdges = []; + var lastNodeIndex = Array.FindIndex(NewOrderNodes, n => n.NodeId == LastNodeId); + lastNodeIndex = lastNodeIndex != -1 ? lastNodeIndex : 0; + if (lastNodeIndex < NewOrderNodes.Length - 1) pathEdges = [.. SplitChecking(NewOrderNodes[lastNodeIndex], NewOrderNodes[lastNodeIndex + 1], NewOrderEdges[lastNodeIndex])]; + if (lastNodeIndex < NewOrderNodes.Length - 2) + { + var nodes = NewOrderNodes.ToList().GetRange(lastNodeIndex + 1, NewOrderNodes.Length - lastNodeIndex - 1); + var edges = NewOrderEdges.ToList().GetRange(lastNodeIndex + 1, nodes.Count - 1); + for (int i = 0; i < nodes.Count - 1; i++) + { + pathEdges.Add(new() + { + StartX = nodes[i].NodePosition.X, + StartY = nodes[i].NodePosition.Y, + EndX = nodes[i + 1].NodePosition.X, + EndY = nodes[i + 1].NodePosition.Y, + ControlPoint1X = edges[i].Trajectory is not null && edges[i].Trajectory.ControlPoints.Length > 2 ? edges[i].Trajectory.ControlPoints[1].X : 0, + ControlPoint1Y = edges[i].Trajectory is not null && edges[i].Trajectory.ControlPoints.Length > 2 ? edges[i].Trajectory.ControlPoints[1].Y : 0, + ControlPoint2X = edges[i].Trajectory is not null && edges[i].Trajectory.ControlPoints.Length > 3 ? edges[i].Trajectory.ControlPoints[2].X : 0, + ControlPoint2Y = edges[i].Trajectory is not null && edges[i].Trajectory.ControlPoints.Length > 3 ? edges[i].Trajectory.ControlPoints[2].Y : 0, + Degree = edges[i].Trajectory.Degree, + }); + } + } + return (NodeStates, [.. pathEdges]); + } + } diff --git a/RobotApp/Services/Robot/RobotPathPlanner.cs b/RobotApp/Services/Robot/RobotPathPlanner.cs index 8055510..33f7238 100644 --- a/RobotApp/Services/Robot/RobotPathPlanner.cs +++ b/RobotApp/Services/Robot/RobotPathPlanner.cs @@ -164,4 +164,72 @@ public class RobotPathPlanner(IConfiguration Configuration) } return [.. navigationNode]; } + + public static Node[] PathSplit(Node[] nodes, Edge[] edges, double resolutionSplit) + { + if (nodes.Length < 2) throw new PathPlannerException(RobotErrors.Error1002(nodes.Length)); + if (edges.Length < 1) throw new PathPlannerException(RobotErrors.Error1003(edges.Length)); + if (edges.Length != nodes.Length - 1) throw new PathPlannerException(RobotErrors.Error1004(nodes.Length, edges.Length)); + + List navigationNode = [new() + { + NodeId = nodes[0].NodeId, + NodePosition = new() + { + X = nodes[0].NodePosition.X, + Y = nodes[0].NodePosition.Y, + Theta = nodes[0].NodePosition.Theta, + } + }]; + foreach (var edge in edges) + { + var startNode = nodes.FirstOrDefault(n => n.NodeId == edge.StartNodeId); + var endNode = nodes.FirstOrDefault(n => n.NodeId == edge.EndNodeId); + if (startNode is null) throw new PathPlannerException(RobotErrors.Error1014(edge.EdgeId, edge.StartNodeId)); + if (endNode is null) throw new PathPlannerException(RobotErrors.Error1014(edge.EdgeId, edge.EndNodeId)); + + var EdgeCalculatorModel = new EdgeCalculatorModel() + { + X1 = startNode.NodePosition.X, + Y1 = startNode.NodePosition.Y, + X2 = endNode.NodePosition.X, + Y2 = endNode.NodePosition.Y, + ControlPoint1X = edge.Trajectory.ControlPoints.Length > 2 ? edge.Trajectory.ControlPoints[1].X : 0, + ControlPoint1Y = edge.Trajectory.ControlPoints.Length > 2 ? edge.Trajectory.ControlPoints[1].Y : 0, + ControlPoint2X = edge.Trajectory.ControlPoints.Length > 3 ? edge.Trajectory.ControlPoints[2].X : 0, + ControlPoint2Y = edge.Trajectory.ControlPoints.Length > 3 ? edge.Trajectory.ControlPoints[2].Y : 0, + TrajectoryDegree = edge.Trajectory.Degree == 1 ? TrajectoryDegree.One : edge.Trajectory.Degree == 2 ? TrajectoryDegree.Two : TrajectoryDegree.Three + }; + + double length = EdgeCalculatorModel.GetEdgeLength(); + if (length <= 0) continue; + double step = resolutionSplit / length; + + for (double t = step; t <= 1 - step; t += step) + { + (double x, double y) = EdgeCalculatorModel.Curve(t); + navigationNode.Add(new() + { + NodeId = string.Empty, + NodePosition = new() + { + X = x, + Y = y, + Theta = startNode.NodePosition.Theta, + } + }); + } + navigationNode.Add(new() + { + NodeId = endNode.NodeId, + NodePosition = new() + { + X = endNode.NodePosition.X, + Y = endNode.NodePosition.Y, + Theta = endNode.NodePosition.Theta, + } + }); + } + return [.. navigationNode]; + } } diff --git a/RobotApp/Services/RobotMonitorService.cs b/RobotApp/Services/RobotMonitorService.cs new file mode 100644 index 0000000..f3f5e27 --- /dev/null +++ b/RobotApp/Services/RobotMonitorService.cs @@ -0,0 +1,84 @@ +using Microsoft.AspNetCore.SignalR; +using RobotApp.Common.Shares.Dtos; +using RobotApp.Hubs; +using RobotApp.Interfaces; +using RobotApp.VDA5050.State; + +namespace RobotApp.Services; + +public class RobotMonitorService(IHubContext HubContext, + IOrder OrderManager, + ILocalization Localization, + Logger Logger) : BackgroundService +{ + private WatchTimerAsync? UpdateTimer; + private const int UpdateInterval = 200; // 200ms + + private async Task UpdateHandler() + { + try + { + // Lấy vị trí robot từ ILocalization (giống như RobotVisualization) + var robotPosition = new RobotPositionDto + { + X = Localization.X, + Y = Localization.Y, + Theta = Localization.Theta + }; + + // Kiểm tra có order không + bool hasOrder = OrderManager.NodeStates.Length > 0 || OrderManager.EdgeStates.Length > 0; + + // Lấy CurrentPath từ IOrder + (NodeState[] nodeStates, EdgeStateDto[] edgeStates) = OrderManager.CurrentPath; + // Tạo DTO + var monitorDto = new RobotMonitorDto + { + RobotPosition = robotPosition, + NodeStates = nodeStates, + EdgeStates = [.. edgeStates], + HasOrder = hasOrder, + Timestamp = DateTime.UtcNow + }; + + // Broadcast qua SignalR Hub + await HubContext.Clients.All.SendAsync("ReceiveRobotMonitorData", monitorDto); + } + catch (Exception ex) + { + Logger.Warning($"Lỗi khi broadcast robot monitor data: {ex.Message}"); + } + } + + protected override async Task ExecuteAsync(CancellationToken stoppingToken) + { + await Task.Yield(); + + // Đợi robot sẵn sàng + while (!stoppingToken.IsCancellationRequested) + { + try + { + // Kiểm tra xem Localization có sẵn sàng không + if (Localization.IsReady) break; + } + catch { } + + await Task.Delay(1000, stoppingToken); + } + + if (!stoppingToken.IsCancellationRequested) + { + UpdateTimer = new(UpdateInterval, UpdateHandler, Logger); + UpdateTimer.Start(); + } + } + + public override Task StopAsync(CancellationToken cancellationToken) + { + UpdateTimer?.Dispose(); + UpdateTimer = null; + return base.StopAsync(cancellationToken); + } +} +