update

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();   

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);
 }

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}");
+                await StopAction();
-                Status = ActionStatus.RUNNING;
-                await StartAction();
             }
-            else if (Status == ActionStatus.RUNNING)
+            else
             {
-                await ExecuteAction();
+                if (Status == ActionStatus.INITIALIZING)
-            }
+                {
-            else if (Status == ActionStatus.PAUSED)
+                    Logger?.Info($"Thực hiện action {Type}");
-            {
+                    Status = ActionStatus.RUNNING;
-                await PauseAction();
+                    await StartAction();
-            }
+                }
-            else if (Status == ActionStatus.WAITING && IsPaused)
+                else if (Status == ActionStatus.RUNNING)
-            {
+                {
-                await ResumeAction();
+                    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);
     }
 }

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<IOrder>();
+        RobotOrder = Scope.ServiceProvider.GetRequiredService<IOrder>();
+        RobotAction = Scope.ServiceProvider.GetRequiredService<IInstantActions>();
         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();
     }
 }

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()

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()

RobotApp/Services/Robot/Actions/RobotMutedBaseOffAction.cs

@@ -9,6 +9,7 @@ public class RobotMutedBaseOffAction(IServiceProvider ServiceProvider) : RobotAc
         Scope ??= ServiceProvider.CreateAsyncScope();
         var RobotSafety = Scope.ServiceProvider.GetRequiredService<ISafety>();
         RobotSafety.SetMutedBase(false);
+        Status = VDA5050.State.ActionStatus.FINISHED;
         return base.StartAction();
     }
 

RobotApp/Services/Robot/Actions/RobotMutedBaseOnAction.cs

@@ -9,6 +9,7 @@ public class RobotMutedBaseOnAction(IServiceProvider ServiceProvider) : RobotAct
         Scope ??= ServiceProvider.CreateAsyncScope();
         var RobotSafety = Scope.ServiceProvider.GetRequiredService<ISafety>();
         RobotSafety.SetMutedBase(true);
+        Status = VDA5050.State.ActionStatus.FINISHED;
         return base.StartAction();
     }
 

RobotApp/Services/Robot/Actions/RobotMutedLoadOffAction.cs

@@ -9,6 +9,7 @@ public class RobotMutedLoadOffAction(IServiceProvider ServiceProvider) : RobotAc
         Scope ??= ServiceProvider.CreateAsyncScope();
         var RobotSafety = Scope.ServiceProvider.GetRequiredService<ISafety>();
         RobotSafety.SetMutedLoad(false);
+        Status = VDA5050.State.ActionStatus.FINISHED;
         return base.StartAction();
     }
 

RobotApp/Services/Robot/Actions/RobotMutedLoadOnAction.cs

@@ -9,6 +9,7 @@ public class RobotMutedLoadOnAction(IServiceProvider ServiceProvider) : RobotAct
         Scope ??= ServiceProvider.CreateAsyncScope();
         var RobotSafety = Scope.ServiceProvider.GetRequiredService<ISafety>();
         RobotSafety.SetMutedLoad(true);
+        Status = VDA5050.State.ActionStatus.FINISHED;
         return base.StartAction();
     }
 

RobotApp/Services/Robot/RobotActionController.cs

@@ -20,12 +20,14 @@ public class RobotActionController(Logger<RobotActionController> Logger, RobotAc
         })];
     public bool HasActionRunning => !ActionQueue.IsEmpty || Actions.Values.Any(a => !a.IsCompleted);
 
-    private readonly ConcurrentDictionary<string, RobotAction> Actions = [];
+    private readonly Dictionary<string, RobotAction> Actions = [];
     private readonly ConcurrentQueue<(ActionScopes scope, VDA5050.InstantAction.Action action)> ActionQueue = [];
 
     private WatchTimer<RobotActionController>? 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<RobotActionController> 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<RobotActionController> 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<RobotActionController> 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<RobotActionController> 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<RobotActionController> Logger, RobotAc
     public void ClearInstantActions()
     {
         ActionQueue.Clear();
+        foreach (var action in Actions.Values)
+        {
+            action.Dispose();
+        }
         Actions.Clear();
     }
 

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<SafetySpeed, double> 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},
+    };
 }

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();
+            if (StateManager.HasState(ExecutingStateType.Moving.ToString()))
-            else if(StateManager.HasState(ExecutingStateType.ACT.ToString())) ActionManager.PauseActions();
+            {
+                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();
+            }
+        }
     }
 }

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()

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;
+    }
 }

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, 
+public class RobotOrderController(INavigation NavigationManager,
-                                  ILocalization Localization, 
+                                  ILocalization Localization,
-                                  IInstantActions ActionManager, 
+                                  IInstantActions ActionManager,
-                                  IError ErrorManager, 
+                                  IError ErrorManager,
+                                  ISafety SafetyManager,
                                   RobotStateMachine StateManager,
+                                  RobotConfiguration RobotConfiguration,
                                   Logger<RobotOrderController> Logger) : IOrder
 {
     public string OrderId { get; private set; } = string.Empty;
     public int OrderUpdateId { get; private set; }
-    public NodeState[] NodeStates => [.. Nodes.Select(n => new NodeState
+    public NodeState[] NodeStates { get; private set; } = [];
-                    {
+    public EdgeState[] EdgeStates { get; private set; } = [];
-                        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 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<RobotOrderController>? OrderTimer;
 
     private readonly Dictionary<string, Action[]> OrderActions = [];
+    private readonly ConcurrentQueue<Action> 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();
+        if (!string.IsNullOrEmpty(OrderId) || OrderTimer is not null) IsCancelOrder = true;
-        NavigationFinished();
     }
 
     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}");
         }
     }
+
 }

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)
         {

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();

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<RobotStateMachine> Logger) : IDisposable
 {
-    private readonly Dictionary<Type, Dictionary<Enum, IRobotState>> StateRegistry = [];
+    private readonly Lock StateLock = new();
+    private readonly ConcurrentDictionary<Type, Dictionary<Enum, IRobotState>> StateRegistry = [];
 
     public IRobotState CurrentState { get; private set; } = null!;
     public event Action<IRobotState?, IRobotState>? OnStateChanged;
@@ -12,8 +14,6 @@ public record RobotStateMachine(Logger<RobotStateMachine> Logger) : IDisposable
     public string CurrentStateName => CurrentState.Name.ToString();
     public string RootStateName => GetRootStateName();
 
-    private readonly Lock StateLock = new();
-
     public void InitializeHierarchyStates()
     {
         if (IsInitialized) return;