From 6eeed8c7b4cc814f99e10fef89ef3442e338c02b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C4=90=C4=83ng=20Nguy=E1=BB=85n?= Date: Fri, 24 Oct 2025 17:09:00 +0700 Subject: [PATCH] update --- RobotApp/Interfaces/IInstantActions.cs | 7 +- RobotApp/Interfaces/INavigation.cs | 2 +- .../Services/Robot/Actions/RobotAction.cs | 60 +++--- .../Robot/Actions/RobotCancelOrderAction.cs | 21 +- .../Robot/Actions/RobotLiftDownAction.cs | 4 +- .../Robot/Actions/RobotLiftUpAction.cs | 4 +- .../Robot/Actions/RobotMutedBaseOffAction.cs | 1 + .../Robot/Actions/RobotMutedBaseOnAction.cs | 1 + .../Robot/Actions/RobotMutedLoadOffAction.cs | 1 + .../Robot/Actions/RobotMutedLoadOnAction.cs | 1 + .../Services/Robot/RobotActionController.cs | 26 ++- RobotApp/Services/Robot/RobotConfiguration.cs | 13 +- RobotApp/Services/Robot/RobotController.cs | 25 ++- RobotApp/Services/Robot/RobotFactsheet.cs | 8 +- RobotApp/Services/Robot/RobotNavigation.cs | 6 +- .../Services/Robot/RobotOrderController.cs | 191 +++++++++++++----- .../Navigation/DifferentialNavigation.cs | 1 + .../Navigation/SimulationNavigation.cs | 6 +- RobotApp/Services/State/RobotStateMachine.cs | 6 +- 19 files changed, 288 insertions(+), 96 deletions(-) diff --git a/RobotApp/Interfaces/IInstantActions.cs b/RobotApp/Interfaces/IInstantActions.cs index d6b6362..67f1397 100644 --- a/RobotApp/Interfaces/IInstantActions.cs +++ b/RobotApp/Interfaces/IInstantActions.cs @@ -1,4 +1,5 @@ -using RobotApp.VDA5050.State; +using RobotApp.Services.Robot.Actions; +using RobotApp.VDA5050.State; using Action = RobotApp.VDA5050.InstantAction.Action; namespace RobotApp.Interfaces; @@ -7,9 +8,11 @@ public interface IInstantActions { ActionState[] ActionStates { get; } bool HasActionRunning { get; } + RobotAction? this[string actionId] { get; } void AddOrderActions(Action[] actions); void AddInstantAction(Action[] action); - void StartOrderAction(string actionId, bool wait); + void StartOrderAction(string actionId, bool wait = false); + void StopOrderAction(string actionId = ""); void ClearInstantActions(); void PauseActions(); void ResumeActions(); diff --git a/RobotApp/Interfaces/INavigation.cs b/RobotApp/Interfaces/INavigation.cs index 278752c..e0c5021 100644 --- a/RobotApp/Interfaces/INavigation.cs +++ b/RobotApp/Interfaces/INavigation.cs @@ -26,7 +26,6 @@ public enum NavigationProccess public interface INavigation { - event Action? OnNavigationStateChanged; event Action? OnNavigationFinished; bool IsReady { get; } bool Driving { get; } @@ -43,4 +42,5 @@ public interface INavigation void RefreshOrder(Node[] nodes, Edge[] edges); void Refresh(); void CancelMovement(); + void SetSpeed(double speed); } diff --git a/RobotApp/Services/Robot/Actions/RobotAction.cs b/RobotApp/Services/Robot/Actions/RobotAction.cs index 4edefce..fa42d4c 100644 --- a/RobotApp/Services/Robot/Actions/RobotAction.cs +++ b/RobotApp/Services/Robot/Actions/RobotAction.cs @@ -1,4 +1,5 @@ using Grpc.Core; +using MudBlazor; using RobotApp.Services.Exceptions; using RobotApp.VDA5050.Factsheet; using RobotApp.VDA5050.InstantAction; @@ -7,7 +8,7 @@ using RobotApp.VDA5050.Type; namespace RobotApp.Services.Robot.Actions; -public abstract class RobotAction(IServiceProvider serviceProvider) : IAsyncDisposable +public abstract class RobotAction(IServiceProvider serviceProvider) : IDisposable { public ActionType Type { get; private set; } public string Id { get; private set; } = ""; @@ -30,6 +31,8 @@ public abstract class RobotAction(IServiceProvider serviceProvider) : IAsyncDisp protected bool IsPaused = false; protected ActionStatus HistoryStatus; + private bool IsCancelAction = false; + public bool Initialize(ActionScopes actionScope, VDA5050.InstantAction.Action action) { Status = ActionStatus.WAITING; @@ -65,7 +68,7 @@ public abstract class RobotAction(IServiceProvider serviceProvider) : IAsyncDisp public void Cancel() { - Dispose(); + if(!IsCompleted) IsCancelAction = true; } public async Task WaitAsync(CancellationToken cancellationToken) @@ -89,6 +92,13 @@ public abstract class RobotAction(IServiceProvider serviceProvider) : IAsyncDisp return Task.CompletedTask; } + protected virtual Task StopAction() + { + Status = ActionStatus.FAILED; + ResultDescription = "Action bị hủy bỏ."; + return Task.CompletedTask; + } + protected virtual Task ExecuteAction() { return Task.CompletedTask; @@ -139,28 +149,35 @@ public abstract class RobotAction(IServiceProvider serviceProvider) : IAsyncDisp { try { - if (Status == ActionStatus.INITIALIZING) + if (IsCancelAction) { - Logger?.Info($"Thực hiện action {Type}"); - Status = ActionStatus.RUNNING; - await StartAction(); + await StopAction(); } - else if (Status == ActionStatus.RUNNING) + else { - await ExecuteAction(); - } - else if (Status == ActionStatus.PAUSED) - { - await PauseAction(); - } - else if (Status == ActionStatus.WAITING && IsPaused) - { - await ResumeAction(); + if (Status == ActionStatus.INITIALIZING) + { + Logger?.Info($"Thực hiện action {Type}"); + Status = ActionStatus.RUNNING; + await StartAction(); + } + else if (Status == ActionStatus.RUNNING) + { + await ExecuteAction(); + } + else if (Status == ActionStatus.PAUSED) + { + await PauseAction(); + } + else if (Status == ActionStatus.WAITING && IsPaused) + { + await ResumeAction(); + } } if (IsCompleted) { - await Dispose(); + Dispose(); } } catch (Exception ex) @@ -171,16 +188,11 @@ public abstract class RobotAction(IServiceProvider serviceProvider) : IAsyncDisp } } - private Task Dispose() + public void Dispose() { + if (!IsCompleted) StopAction(); ActionTimer?.Dispose(); ActionTimer = null; - return Task.CompletedTask; - } - - public async ValueTask DisposeAsync() - { - await Dispose(); GC.SuppressFinalize(this); } } diff --git a/RobotApp/Services/Robot/Actions/RobotCancelOrderAction.cs b/RobotApp/Services/Robot/Actions/RobotCancelOrderAction.cs index c70245b..11b2625 100644 --- a/RobotApp/Services/Robot/Actions/RobotCancelOrderAction.cs +++ b/RobotApp/Services/Robot/Actions/RobotCancelOrderAction.cs @@ -1,20 +1,37 @@ using RobotApp.Interfaces; +using RobotApp.VDA5050.State; namespace RobotApp.Services.Robot.Actions; public class RobotCancelOrderAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) { + private IOrder? RobotOrder; + private IInstantActions? RobotAction; protected override Task StartAction() { Scope ??= ServiceProvider.CreateAsyncScope(); - var RobotOrder = Scope.ServiceProvider.GetRequiredService(); + RobotOrder = Scope.ServiceProvider.GetRequiredService(); + RobotAction = Scope.ServiceProvider.GetRequiredService(); RobotOrder.StopOrder(); - Status = VDA5050.State.ActionStatus.FINISHED; + RobotAction.StopOrderAction(); return base.StartAction(); } protected override Task ExecuteAction() { + if (RobotOrder is null || RobotAction is null) + { + Status = ActionStatus.FAILED; + ResultDescription = $"Không thể tìm thấy module quản lý {(RobotOrder is null ? "Order" : RobotAction is null ? "Action" : "")}"; + } + else + { + if (string.IsNullOrEmpty(RobotOrder.OrderId) && !RobotAction.HasActionRunning) + { + Status = ActionStatus.FINISHED; + ResultDescription = AgvAction is null ? ResultDescription : AgvAction.ResultDescription; + } + } return base.ExecuteAction(); } } diff --git a/RobotApp/Services/Robot/Actions/RobotLiftDownAction.cs b/RobotApp/Services/Robot/Actions/RobotLiftDownAction.cs index b5eaddc..74e8c27 100644 --- a/RobotApp/Services/Robot/Actions/RobotLiftDownAction.cs +++ b/RobotApp/Services/Robot/Actions/RobotLiftDownAction.cs @@ -2,11 +2,11 @@ public class RobotLiftDownAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) { - protected override Task StartAction() + protected override async Task StartAction() { + await Task.Delay(2000); Status = VDA5050.State.ActionStatus.FINISHED; ResultDescription = AgvAction is null ? ResultDescription : AgvAction.ResultDescription; - return base.StartAction(); } protected override Task ExecuteAction() diff --git a/RobotApp/Services/Robot/Actions/RobotLiftUpAction.cs b/RobotApp/Services/Robot/Actions/RobotLiftUpAction.cs index e261142..dc74829 100644 --- a/RobotApp/Services/Robot/Actions/RobotLiftUpAction.cs +++ b/RobotApp/Services/Robot/Actions/RobotLiftUpAction.cs @@ -2,11 +2,11 @@ public class RobotLiftUpAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) { - protected override Task StartAction() + protected override async Task StartAction() { + await Task.Delay(2000); Status = VDA5050.State.ActionStatus.FINISHED; ResultDescription = AgvAction is null ? ResultDescription : AgvAction.ResultDescription; - return base.StartAction(); } protected override Task ExecuteAction() diff --git a/RobotApp/Services/Robot/Actions/RobotMutedBaseOffAction.cs b/RobotApp/Services/Robot/Actions/RobotMutedBaseOffAction.cs index b86bf8f..742bb98 100644 --- a/RobotApp/Services/Robot/Actions/RobotMutedBaseOffAction.cs +++ b/RobotApp/Services/Robot/Actions/RobotMutedBaseOffAction.cs @@ -9,6 +9,7 @@ public class RobotMutedBaseOffAction(IServiceProvider ServiceProvider) : RobotAc Scope ??= ServiceProvider.CreateAsyncScope(); var RobotSafety = Scope.ServiceProvider.GetRequiredService(); RobotSafety.SetMutedBase(false); + Status = VDA5050.State.ActionStatus.FINISHED; return base.StartAction(); } diff --git a/RobotApp/Services/Robot/Actions/RobotMutedBaseOnAction.cs b/RobotApp/Services/Robot/Actions/RobotMutedBaseOnAction.cs index 0e4925d..787d33e 100644 --- a/RobotApp/Services/Robot/Actions/RobotMutedBaseOnAction.cs +++ b/RobotApp/Services/Robot/Actions/RobotMutedBaseOnAction.cs @@ -9,6 +9,7 @@ public class RobotMutedBaseOnAction(IServiceProvider ServiceProvider) : RobotAct Scope ??= ServiceProvider.CreateAsyncScope(); var RobotSafety = Scope.ServiceProvider.GetRequiredService(); RobotSafety.SetMutedBase(true); + Status = VDA5050.State.ActionStatus.FINISHED; return base.StartAction(); } diff --git a/RobotApp/Services/Robot/Actions/RobotMutedLoadOffAction.cs b/RobotApp/Services/Robot/Actions/RobotMutedLoadOffAction.cs index caed90a..6bba15e 100644 --- a/RobotApp/Services/Robot/Actions/RobotMutedLoadOffAction.cs +++ b/RobotApp/Services/Robot/Actions/RobotMutedLoadOffAction.cs @@ -9,6 +9,7 @@ public class RobotMutedLoadOffAction(IServiceProvider ServiceProvider) : RobotAc Scope ??= ServiceProvider.CreateAsyncScope(); var RobotSafety = Scope.ServiceProvider.GetRequiredService(); RobotSafety.SetMutedLoad(false); + Status = VDA5050.State.ActionStatus.FINISHED; return base.StartAction(); } diff --git a/RobotApp/Services/Robot/Actions/RobotMutedLoadOnAction.cs b/RobotApp/Services/Robot/Actions/RobotMutedLoadOnAction.cs index fcbde1b..751f126 100644 --- a/RobotApp/Services/Robot/Actions/RobotMutedLoadOnAction.cs +++ b/RobotApp/Services/Robot/Actions/RobotMutedLoadOnAction.cs @@ -9,6 +9,7 @@ public class RobotMutedLoadOnAction(IServiceProvider ServiceProvider) : RobotAct Scope ??= ServiceProvider.CreateAsyncScope(); var RobotSafety = Scope.ServiceProvider.GetRequiredService(); RobotSafety.SetMutedLoad(true); + Status = VDA5050.State.ActionStatus.FINISHED; return base.StartAction(); } diff --git a/RobotApp/Services/Robot/RobotActionController.cs b/RobotApp/Services/Robot/RobotActionController.cs index 2e8c21a..1271439 100644 --- a/RobotApp/Services/Robot/RobotActionController.cs +++ b/RobotApp/Services/Robot/RobotActionController.cs @@ -20,12 +20,14 @@ public class RobotActionController(Logger Logger, RobotAc })]; public bool HasActionRunning => !ActionQueue.IsEmpty || Actions.Values.Any(a => !a.IsCompleted); - private readonly ConcurrentDictionary Actions = []; + private readonly Dictionary Actions = []; private readonly ConcurrentQueue<(ActionScopes scope, VDA5050.InstantAction.Action action)> ActionQueue = []; private WatchTimer? HandlerTimer; private const int HandlerInterval = 200; + public RobotAction? this[string actionId] => Actions.TryGetValue(actionId, out RobotAction? action) && action is not null ? action : null; + public void AddInstantAction(VDA5050.InstantAction.Action[] actions) { foreach (var action in actions) @@ -42,7 +44,7 @@ public class RobotActionController(Logger Logger, RobotAc } } - public void StartOrderAction(string actionId, bool wait) + public void StartOrderAction(string actionId, bool wait = false) { if (Actions.TryGetValue(actionId, out RobotAction? robotAction) && robotAction is not null) { @@ -54,6 +56,18 @@ public class RobotActionController(Logger Logger, RobotAc } } + public void StopOrderAction(string actionId = "") + { + if(string.IsNullOrEmpty(actionId)) + { + foreach (var action in Actions.Values) + { + if (!action.IsCompleted) action.Cancel(); + } + } + else if (Actions.TryGetValue(actionId, out RobotAction? robotAction) && robotAction is not null) robotAction.Cancel(); + } + public void PauseActions() { foreach(var action in Actions.Values) @@ -78,7 +92,7 @@ public class RobotActionController(Logger Logger, RobotAc { if (ActionQueue.TryDequeue(out var result)) { - if (Actions.ContainsKey(result.action.ActionId)) return; + if (Actions.ContainsKey(result.action.ActionId)) continue; if (Enum.TryParse(result.action.ActionType, out ActionType actionType)) { var robotAction = ActionStorage[actionType]; @@ -87,7 +101,7 @@ public class RobotActionController(Logger Logger, RobotAc var init = robotAction.Initialize(result.scope, result.action); if (init) { - Actions.TryAdd(result.action.ActionId, robotAction); + Actions.Add(result.action.ActionId, robotAction); if(result.scope == ActionScopes.INSTANT) robotAction.Start(); } } @@ -113,6 +127,10 @@ public class RobotActionController(Logger Logger, RobotAc public void ClearInstantActions() { ActionQueue.Clear(); + foreach (var action in Actions.Values) + { + action.Dispose(); + } Actions.Clear(); } diff --git a/RobotApp/Services/Robot/RobotConfiguration.cs b/RobotApp/Services/Robot/RobotConfiguration.cs index 2854871..6801e3b 100644 --- a/RobotApp/Services/Robot/RobotConfiguration.cs +++ b/RobotApp/Services/Robot/RobotConfiguration.cs @@ -1,5 +1,7 @@ using RobotApp.Common.Shares.Enums; +using RobotApp.Interfaces; using RobotApp.Services.Robot.Simulation; +using RobotApp.VDA5050.State; namespace RobotApp.Services.Robot; @@ -22,6 +24,15 @@ public class RobotConfiguration public byte PLCUnitId { get; set; } = 1; public bool IsSimulation { get; set; } = true; public SimulationModel SimulationModel { get; set; } = new(); - public string XlocAddress { get; set; } = "http://192.168.195.56:50050"; + public Dictionary SafetySpeedMap = new () + { + { SafetySpeed.Very_Slow, 0.15}, + { SafetySpeed.Slow, 0.3}, + { SafetySpeed.Normal, 0.6}, + { SafetySpeed.Medium, 0.9}, + { SafetySpeed.Optimal, 1.2}, + { SafetySpeed.Fast, 1.5}, + { SafetySpeed.Very_Fast, 1.9}, + }; } diff --git a/RobotApp/Services/Robot/RobotController.cs b/RobotApp/Services/Robot/RobotController.cs index 376cca1..fff62f5 100644 --- a/RobotApp/Services/Robot/RobotController.cs +++ b/RobotApp/Services/Robot/RobotController.cs @@ -101,13 +101,32 @@ public partial class RobotController(IOrder OrderManager, { if(StateManager.HasState(AutoStateType.Executing.ToString())) { - if(StateManager.HasState(ExecutingStateType.Moving.ToString())) OrderManager.PauseOrder(); - else if(StateManager.HasState(ExecutingStateType.ACT.ToString())) ActionManager.PauseActions(); + if (StateManager.HasState(ExecutingStateType.Moving.ToString())) + { + OrderManager.PauseOrder(); + StateManager.TransitionTo(AutoStateType.Paused); + } + else if (StateManager.HasState(ExecutingStateType.ACT.ToString())) + { + ActionManager.PauseActions(); + StateManager.TransitionTo(AutoStateType.Paused); + } } } public void Resume() { - + if(StateManager.HasState(AutoStateType.Paused.ToString())) + { + StateManager.TransitionTo(AutoStateType.Executing); + if (StateManager.HasState(ExecutingStateType.Moving.ToString())) + { + OrderManager.ResumeOrder(); + } + else if (StateManager.HasState(ExecutingStateType.ACT.ToString())) + { + ActionManager.ResumeActions(); + } + } } } diff --git a/RobotApp/Services/Robot/RobotFactsheet.cs b/RobotApp/Services/Robot/RobotFactsheet.cs index e1a43ae..0e4aa44 100644 --- a/RobotApp/Services/Robot/RobotFactsheet.cs +++ b/RobotApp/Services/Robot/RobotFactsheet.cs @@ -241,7 +241,7 @@ public class RobotFactsheet(RobotConnection RobotConnection, RobotConfiguration ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()], ActionParameters = [], ResultDescription = "Robot đã bật chế độ muted base.", - BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString()], + BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString(), BlockingType.HARD.ToString()], }; public readonly static AgvAction MutedBaseOff = new() @@ -251,7 +251,7 @@ public class RobotFactsheet(RobotConnection RobotConnection, RobotConfiguration ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()], ActionParameters = [], ResultDescription = "Robot đã tắt chế độ muted base.", - BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString()], + BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString(), BlockingType.HARD.ToString()], }; public readonly static AgvAction MutedLoadOn = new() @@ -261,7 +261,7 @@ public class RobotFactsheet(RobotConnection RobotConnection, RobotConfiguration ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()], ActionParameters = [], ResultDescription = "Robot đã bật chế độ muted load.", - BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString()], + BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString(), BlockingType.HARD.ToString()], }; public readonly static AgvAction MutedLoadOff = new() @@ -271,7 +271,7 @@ public class RobotFactsheet(RobotConnection RobotConnection, RobotConfiguration ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()], ActionParameters = [], ResultDescription = "Robot đã tắt chế độ muted load.", - BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString()], + BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString(), BlockingType.HARD.ToString()], }; public readonly static AgvAction DockTo = new() diff --git a/RobotApp/Services/Robot/RobotNavigation.cs b/RobotApp/Services/Robot/RobotNavigation.cs index adcd22a..4eb33cd 100644 --- a/RobotApp/Services/Robot/RobotNavigation.cs +++ b/RobotApp/Services/Robot/RobotNavigation.cs @@ -16,7 +16,6 @@ public class RobotNavigation(RobotConfiguration RobotConfiguration, IServiceProv private SimulationNavigation? SimNavigation; private readonly bool IsSimulation = RobotConfiguration.IsSimulation; - public event Action? OnNavigationStateChanged; public event Action? OnNavigationFinished; public void CancelMovement() @@ -85,4 +84,9 @@ public class RobotNavigation(RobotConfiguration RobotConfiguration, IServiceProv OnNavigationFinished?.Invoke(); if(SimNavigation is not null) SimNavigation.OnNavigationFinished -= NavigationFinished; } + + public void SetSpeed(double speed) + { + return; + } } diff --git a/RobotApp/Services/Robot/RobotOrderController.cs b/RobotApp/Services/Robot/RobotOrderController.cs index 1783aeb..d7e2bf8 100644 --- a/RobotApp/Services/Robot/RobotOrderController.cs +++ b/RobotApp/Services/Robot/RobotOrderController.cs @@ -1,45 +1,28 @@ -using Microsoft.AspNetCore.Components; -using RobotApp.Common.Shares.Enums; +using RobotApp.Common.Shares.Enums; using RobotApp.Interfaces; using RobotApp.Services.Exceptions; using RobotApp.Services.State; +using RobotApp.VDA5050.InstantAction; using RobotApp.VDA5050.Order; using RobotApp.VDA5050.State; +using System.Collections.Concurrent; using Action = RobotApp.VDA5050.InstantAction.Action; namespace RobotApp.Services.Robot; -public class RobotOrderController(INavigation NavigationManager, - ILocalization Localization, - IInstantActions ActionManager, - IError ErrorManager, +public class RobotOrderController(INavigation NavigationManager, + ILocalization Localization, + IInstantActions ActionManager, + IError ErrorManager, + ISafety SafetyManager, RobotStateMachine StateManager, + RobotConfiguration RobotConfiguration, Logger Logger) : IOrder { public string OrderId { get; private set; } = string.Empty; public int OrderUpdateId { get; private set; } - public NodeState[] NodeStates => [.. Nodes.Select(n => new NodeState - { - NodeId = n.NodeId, - Released = n.Released, - SequenceId = n.SequenceId, - NodeDescription = n.NodeDescription, - NodePosition = new() - { - X = n.NodePosition.X, - Y = n.NodePosition.Y, - Theta = n.NodePosition.Theta, - MapId = n.NodePosition.MapId - } - })]; - public EdgeState[] EdgeStates => [.. Edges.Select(e => new EdgeState - { - EdgeId = e.EdgeId, - Released = e.Released, - EdgeDescription = e.EdgeDescription, - SequenceId = e.SequenceId, - Trajectory = e.Trajectory - })]; + public NodeState[] NodeStates { get; private set; } = []; + public EdgeState[] EdgeStates { get; private set; } = []; public string LastNodeId => LastNode is null ? "" : LastNode.NodeId; public int LastNodeSequenceId => LastNode is null ? 0 : LastNode.SequenceId; @@ -47,13 +30,20 @@ public class RobotOrderController(INavigation NavigationManager, private WatchTimer? OrderTimer; private readonly Dictionary OrderActions = []; + private readonly ConcurrentQueue ActionWaitingRunning = []; + private OrderMsg? NewOrder; private Node[] Nodes = []; private Edge[] Edges = []; private Node? CurrentBaseNode; private Node? LastNode; + private readonly Lock LockObject = new(); - private OrderMsg? NewOrder; + + private bool IsCancelOrder = false; + private bool IsActionRunning = false; + private bool IsWaitingPaused = false; + private Action? ActionHard = null; public void UpdateOrder(OrderMsg order) { @@ -66,8 +56,7 @@ public class RobotOrderController(INavigation NavigationManager, public void StopOrder() { - NavigationManager.CancelMovement(); - NavigationFinished(); + if (!string.IsNullOrEmpty(OrderId) || OrderTimer is not null) IsCancelOrder = true; } public void PauseOrder() @@ -95,13 +84,13 @@ public class RobotOrderController(INavigation NavigationManager, private Node? GetCurrentNode() { Node? inNode = null; - double minDistance = double.MaxValue; + double minDistance = double.MaxValue; foreach (var node in Nodes) { var distance = Localization.DistanceTo(node.NodePosition.X, node.NodePosition.Y); - if(distance <= node.NodePosition.AllowedDeviationXY) + if (distance <= node.NodePosition.AllowedDeviationXY) { - if(distance < minDistance) + if (distance < minDistance) { minDistance = distance; inNode = node; @@ -114,43 +103,89 @@ public class RobotOrderController(INavigation NavigationManager, private void NavigationFinished() { HandleOrderStop(); + SafetyManager.OnSafetySpeedChanged -= OnSafetySpeedChanged; OrderId = string.Empty; OrderUpdateId = 0; OrderActions.Clear(); + ActionWaitingRunning.Clear(); CurrentBaseNode = null; Nodes = []; Edges = []; StateManager.TransitionTo(AutoStateType.Idle); } - private void HanleNewOrder(OrderMsg order) + private void OnSafetySpeedChanged(SafetySpeed safetySpeed) + { + double speed = 0.15; + if (RobotConfiguration.SafetySpeedMap.TryGetValue(safetySpeed, out double safeSpeed)) speed = safeSpeed; + NavigationManager.SetSpeed(speed); + } + + private void UpdateState() + { + NodeStates = [.. Nodes.Select(n => new NodeState + { + NodeId = n.NodeId, + Released = n.Released, + SequenceId = n.SequenceId, + NodeDescription = n.NodeDescription, + NodePosition = new() + { + X = n.NodePosition.X, + Y = n.NodePosition.Y, + Theta = n.NodePosition.Theta, + MapId = n.NodePosition.MapId + } + })]; + EdgeStates = [.. Edges.Select(e => new EdgeState + { + EdgeId = e.EdgeId, + Released = e.Released, + EdgeDescription = e.EdgeDescription, + SequenceId = e.SequenceId, + Trajectory = e.Trajectory + })]; + } + + private void HandleNewOrder(OrderMsg order) { if (NavigationManager.State != NavigationState.Idle) throw new OrderException(RobotErrors.Error1012(NavigationManager.State)); + OrderActions.Clear(); for (int i = 0; i < order.Nodes.Length; i++) { if (order.Nodes[i].Actions is not null && order.Nodes[i].Actions.Length > 0) { foreach (var item in order.Nodes[i].Actions) { - item.ActionDescription += $"\n NodeId: {order.Nodes[i].NodeId}"; + item.ActionDescription += $".On NodeId: {order.Nodes[i].NodeId}"; } - OrderActions.Add(order.Nodes[i].NodeId, order.Nodes[i].Actions); + if (OrderActions.TryGetValue(order.Nodes[i].NodeId, out Action[]? actions) && actions is not null) + { + actions = [.. actions, .. order.Nodes[i].Actions]; + } + else OrderActions.Add(order.Nodes[i].NodeId, order.Nodes[i].Actions); } - if (i < order.Nodes.Length - 1 && order.Edges[i] is not null && order.Edges[i].Length > 0) + if (i < order.Nodes.Length - 1 && order.Edges[i].Actions is not null && order.Edges[i].Actions.Length > 0) { foreach (var item in order.Edges[i].Actions) { - item.ActionDescription += $"\n NodeId: {order.Nodes[i].NodeId}"; + item.ActionDescription += $".On NodeId: {order.Nodes[i].NodeId}"; } - OrderActions.TryAdd(order.Nodes[i].NodeId, order.Edges[i].Actions); + if (OrderActions.TryGetValue(order.Nodes[i].NodeId, out Action[]? actions) && actions is not null) + { + actions = [.. actions, .. order.Edges[i].Actions]; + } + else OrderActions.Add(order.Nodes[i].NodeId, order.Edges[i].Actions); } if (order.Nodes[i].SequenceId != i) throw new OrderException(RobotErrors.Error1010(order.Nodes[i].NodeId, order.Nodes[i].SequenceId, i)); if (i < order.Nodes.Length - 1 && order.Edges[i].SequenceId != i) throw new OrderException(RobotErrors.Error1011(order.Edges[i].EdgeId, order.Edges[i].SequenceId, i)); if (order.Nodes[i].Released) CurrentBaseNode = order.Nodes[i]; } - ActionManager.ClearInstantActions(); + + SafetyManager.OnSafetySpeedChanged += OnSafetySpeedChanged; + if (OrderActions.Count > 0) ActionManager.AddOrderActions([.. OrderActions.Values.SelectMany(a => a)]); NavigationManager.Move(order.Nodes, order.Edges); NavigationManager.OnNavigationFinished += NavigationFinished; @@ -159,7 +194,8 @@ public class RobotOrderController(INavigation NavigationManager, Nodes = order.Nodes; Edges = order.Edges; if (CurrentBaseNode is not null) NavigationManager.UpdateOrder(CurrentBaseNode.NodeId); - if(StateManager.CurrentStateName != AutoStateType.Executing.ToString()) StateManager.TransitionTo(AutoStateType.Executing); + if (StateManager.CurrentStateName != AutoStateType.Executing.ToString()) StateManager.TransitionTo(AutoStateType.Executing); + UpdateState(); } private bool IsNewPath() @@ -167,6 +203,18 @@ public class RobotOrderController(INavigation NavigationManager, return true; } + private void ClearLastNode() + { + if (LastNode is null) return; + var currentLastNodeIndex = Array.FindIndex(Nodes, n => n.NodeId == LastNode.NodeId.ToString()); + if (currentLastNodeIndex != -1 && currentLastNodeIndex < Nodes.Length - 1) + { + Nodes = [.. Nodes.Skip(currentLastNodeIndex + 1)]; + Edges = [.. Edges.Skip(currentLastNodeIndex + 1)]; + UpdateState(); + } + } + private void HandleUpdateOrder(OrderMsg order) { if (order.OrderId != OrderId) throw new OrderException(RobotErrors.Error1001(OrderId, order.OrderId)); @@ -181,15 +229,66 @@ public class RobotOrderController(INavigation NavigationManager, Nodes = order.Nodes; Edges = order.Edges; OrderUpdateId = order.OrderUpdateId; + ClearLastNode(); } private void HandleOrder() { + if (IsCancelOrder) + { + IsCancelOrder = false; + NavigationManager.CancelMovement(); + NavigationFinished(); + return; + } + var currentNode = GetCurrentNode(); if (currentNode is not null && currentNode.NodeId != LastNode?.NodeId) { LastNode = currentNode; - // xuử lí nếu gặp node mới + ClearLastNode(); + + if (OrderActions.TryGetValue(currentNode.NodeId, out Action[]? actions) && actions is not null && actions.Length > 0) + { + if (actions.Any(a => a.BlockingType == BlockingType.SOFT.ToString() || a.BlockingType == BlockingType.HARD.ToString())) + { + NavigationManager.Pause(); + IsWaitingPaused = true; + } + foreach (var action in actions) + { + ActionWaitingRunning.Enqueue(action); + } + } + } + + if (ActionHard is not null) + { + var robotAction = ActionManager[ActionHard.ActionId]; + if (robotAction is null || (robotAction is not null && robotAction.IsCompleted)) + { + NavigationManager.Resume(); + ActionHard = null; + } + } + else + { + if (!ActionWaitingRunning.IsEmpty) + { + IsActionRunning = !IsWaitingPaused || (IsWaitingPaused && NavigationManager.State == NavigationState.Paused); + if (IsActionRunning) + { + if (ActionWaitingRunning.TryDequeue(out Action? action) && action is not null) + { + ActionManager.StartOrderAction(action.ActionId); + ActionHard = action.BlockingType == BlockingType.HARD.ToString() ? action : null; + } + } + } + else + { + if (IsWaitingPaused) IsWaitingPaused = false; + } } } @@ -199,7 +298,6 @@ public class RobotOrderController(INavigation NavigationManager, { if (NewOrder is not null) { - Console.WriteLine("Has new Order"); OrderMsg NewOrderHandler; lock (LockObject) { @@ -211,12 +309,12 @@ public class RobotOrderController(INavigation NavigationManager, if (NewOrderHandler.Edges.Length < 1) throw new OrderException(RobotErrors.Error1003(NewOrderHandler.Edges.Length)); if (NewOrderHandler.Edges.Length != NewOrderHandler.Nodes.Length - 1) throw new OrderException(RobotErrors.Error1004(NewOrderHandler.Nodes.Length, NewOrderHandler.Edges.Length)); - if (string.IsNullOrEmpty(OrderId)) HanleNewOrder(NewOrderHandler); + if (string.IsNullOrEmpty(OrderId)) HandleNewOrder(NewOrderHandler); else HandleUpdateOrder(NewOrderHandler); } HandleOrder(); } - catch (RobotExeption orEx ) + catch (RobotExeption orEx) { if (orEx.Error is not null) { @@ -230,4 +328,5 @@ public class RobotOrderController(INavigation NavigationManager, Logger.Warning($"Lỗi khi xử lí Order: {ex.Message}"); } } + } diff --git a/RobotApp/Services/Robot/Simulation/Navigation/DifferentialNavigation.cs b/RobotApp/Services/Robot/Simulation/Navigation/DifferentialNavigation.cs index 30de325..1540878 100644 --- a/RobotApp/Services/Robot/Simulation/Navigation/DifferentialNavigation.cs +++ b/RobotApp/Services/Robot/Simulation/Navigation/DifferentialNavigation.cs @@ -68,6 +68,7 @@ public class DifferentialNavigation : SimulationNavigation } } } + else if (NavState == NavigationState.Paused) VelocityController.Stop(); } catch (Exception ex) { diff --git a/RobotApp/Services/Robot/Simulation/Navigation/SimulationNavigation.cs b/RobotApp/Services/Robot/Simulation/Navigation/SimulationNavigation.cs index a517259..7b433a5 100644 --- a/RobotApp/Services/Robot/Simulation/Navigation/SimulationNavigation.cs +++ b/RobotApp/Services/Robot/Simulation/Navigation/SimulationNavigation.cs @@ -16,7 +16,6 @@ public class SimulationNavigation : INavigation, IDisposable public double VelocityY => Visualization.Vy; public double Omega => Visualization.Omega; - public event Action? OnNavigationStateChanged; public event Action? OnNavigationFinished; protected NavigationState NavState = NavigationState.Idle; @@ -182,6 +181,11 @@ public class SimulationNavigation : INavigation, IDisposable throw new NotImplementedException(); } + public void SetSpeed(double speed) + { + throw new NotImplementedException(); + } + public void Dispose() { HandleNavigationStop(); diff --git a/RobotApp/Services/State/RobotStateMachine.cs b/RobotApp/Services/State/RobotStateMachine.cs index 6e3754a..d6085f3 100644 --- a/RobotApp/Services/State/RobotStateMachine.cs +++ b/RobotApp/Services/State/RobotStateMachine.cs @@ -1,10 +1,12 @@ using RobotApp.Common.Shares.Enums; +using System.Collections.Concurrent; namespace RobotApp.Services.State; public record RobotStateMachine(Logger Logger) : IDisposable { - private readonly Dictionary> StateRegistry = []; + private readonly Lock StateLock = new(); + private readonly ConcurrentDictionary> StateRegistry = []; public IRobotState CurrentState { get; private set; } = null!; public event Action? OnStateChanged; @@ -12,8 +14,6 @@ public record RobotStateMachine(Logger Logger) : IDisposable public string CurrentStateName => CurrentState.Name.ToString(); public string RootStateName => GetRootStateName(); - private readonly Lock StateLock = new(); - public void InitializeHierarchyStates() { if (IsInitialized) return;