update connect synaos

RobotApp/Interfaces/ILoad.cs

@@ -5,4 +5,7 @@ namespace RobotApp.Interfaces;
 public interface ILoad
 {
     Load[] Load { get; }
+
+    void AddLoad();
+    void ClearLoad();
 }

RobotApp/Services/MQTTClient.cs

@@ -37,12 +37,12 @@ public class MQTTClient : IAsyncDisposable
         MqttClientFactory = new MqttClientFactory();
         MqttClientOptions = MqttClientFactory.CreateClientOptionsBuilder()
                             .WithTcpServer(VDA5050Setting.HostServer, VDA5050Setting.Port)
-                            .WithClientId(ClientId)
+                            .WithClientId($"{ClientId}")
                             .WithCleanSession(true)
                             .Build();
         MqttClientSubscribeOptions = MqttClientFactory.CreateSubscribeOptionsBuilder()
-                            .WithTopicFilter(f => f.WithTopic(OrderTopic))
+                            .WithTopicFilter(f => f.WithTopic(OrderTopic).WithAtLeastOnceQoS().WithExactlyOnceQoS().WithAtMostOnceQoS())
-                            .WithTopicFilter(f => f.WithTopic(InstanceActionsTopic))
+                            .WithTopicFilter(f => f.WithTopic(InstanceActionsTopic).WithAtLeastOnceQoS().WithExactlyOnceQoS().WithAtMostOnceQoS())
                             .Build();
     }
 
@@ -146,6 +146,10 @@ public class MQTTClient : IAsyncDisposable
             await CleanupCurrentClient();
 
             MqttClient = MqttClientFactory.CreateMqttClient();
+
+            //MqttClient.ApplicationMessageReceivedAsync -= OnMessageReceived;
+            MqttClient.ApplicationMessageReceivedAsync += OnMessageReceived;
+            //MqttClient.DisconnectedAsync -= OnDisconnected;
             MqttClient.DisconnectedAsync += OnDisconnected;
             while (!cancellationToken.IsCancellationRequested)
             {
@@ -156,7 +160,7 @@ public class MQTTClient : IAsyncDisposable
                         Logger.Warning($"Không thể kết nối tới broker do: {connection.ReasonString}");
                     else
                     {
-                        Logger.Info("Kết nối tới broker thành công");
+                        Logger.Info($"Kết nối tới broker {VDA5050Setting.HostServer} thành công");
                         break;
                     }
                 }
@@ -218,7 +222,7 @@ public class MQTTClient : IAsyncDisposable
                     .WithCleanSession(true);
         if (enablePassword)
         {
-            builder.WithCredentials(VDA5050Setting.UserName, VDA5050Setting.Password);
+            builder = builder.WithCredentials(VDA5050Setting.UserName, VDA5050Setting.Password);
         }
 
         if (enableTls)
@@ -226,10 +230,10 @@ public class MQTTClient : IAsyncDisposable
             var tlsOptions = new MqttClientTlsOptionsBuilder()
                             .UseTls(true)
                             .WithSslProtocols(System.Security.Authentication.SslProtocols.Tls12 | System.Security.Authentication.SslProtocols.Tls13)
-                            .WithCertificateValidationHandler(ValidateCertificates)
+                            //.WithCertificateValidationHandler(ValidateCertificates)
                             .WithClientCertificatesProvider(new MQTTClientCertificatesProvider(VDA5050Setting.CerFile, VDA5050Setting.KeyFile))
                             .Build();
-            builder.WithTlsOptions(tlsOptions);
+            builder =  builder.WithTlsOptions(tlsOptions);
         }
         MqttClientOptions = builder.Build();
     }
@@ -238,6 +242,7 @@ public class MQTTClient : IAsyncDisposable
     {
         try
         {
+            Logger.Info($"Has new message: {args.ApplicationMessage.Topic}");
             if (IsDisposed) return Task.CompletedTask;
 
             var stringData = Encoding.UTF8.GetString(args.ApplicationMessage.Payload);
@@ -255,13 +260,11 @@ public class MQTTClient : IAsyncDisposable
 
     public async Task SubscribeAsync(CancellationToken cancellationToken = default)
     {
-        if (!IsDisposed)
+        //if (!IsDisposed)
-        {
+        //{
             if (MqttClient is null) throw new Exception("Kết nối tới broker chưa được khởi tạo nhưng đã yêu cầu subscribe");
             if (!MqttClient.IsConnected) throw new Exception("Kết nối tới broker chưa thành công nhưng đã yêu cầu subscribe");
 
-            MqttClient.ApplicationMessageReceivedAsync -= OnMessageReceived;
-            MqttClient.ApplicationMessageReceivedAsync += OnMessageReceived;
 
             while (!cancellationToken.IsCancellationRequested)
             {
@@ -295,8 +298,8 @@ public class MQTTClient : IAsyncDisposable
                     await Task.Delay(3000, cancellationToken);
                 }
             }
-        }
+        //}
-        else throw new ObjectDisposedException(nameof(MQTTClient));
+        //else throw new ObjectDisposedException(nameof(MQTTClient));
     }
 
     public async Task<MessageResult> PublishAsync(string topic, string data)

RobotApp/Services/Robot/Actions/RobotCancelOrderAction.cs

@@ -26,7 +26,7 @@ public class RobotCancelOrderAction(IServiceProvider ServiceProvider) : RobotAct
         }
         else
         {
-            if (string.IsNullOrEmpty(RobotOrder.OrderId) && !RobotAction.HasActionRunning)
+            if (RobotOrder.NodeStates.Length == 0 && RobotOrder.EdgeStates.Length == 0 && !RobotAction.HasActionRunning)
             {         
                 Status = ActionStatus.FINISHED;
                 ResultDescription = AgvAction is null ? ResultDescription : AgvAction.ResultDescription;

RobotApp/Services/Robot/Actions/RobotDropAction.cs

@@ -0,0 +1,21 @@
+using RobotApp.Interfaces;
+
+namespace RobotApp.Services.Robot.Actions;
+
+public class RobotDropAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
+{
+    protected override async Task StartAction()
+    {
+        await Task.Delay(2000);
+        Scope ??= ServiceProvider.CreateAsyncScope();
+        var LoadManager = Scope.ServiceProvider.GetRequiredService<ILoad>();
+        LoadManager.ClearLoad();
+        Status = VDA5050.State.ActionStatus.FINISHED;
+        ResultDescription = AgvAction is null ? ResultDescription : AgvAction.ResultDescription;
+    }
+
+    protected override Task ExecuteAction()
+    {
+        return base.ExecuteAction();
+    }
+}

RobotApp/Services/Robot/Actions/RobotPickAction.cs

@@ -0,0 +1,21 @@
+using RobotApp.Interfaces;
+
+namespace RobotApp.Services.Robot.Actions;
+
+public class RobotPickAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
+{
+    protected override async Task StartAction()
+    {
+        await Task.Delay(2000);
+        Scope ??= ServiceProvider.CreateAsyncScope();
+        var LoadManager = Scope.ServiceProvider.GetRequiredService<ILoad>();
+        LoadManager.AddLoad();
+        Status = VDA5050.State.ActionStatus.FINISHED;
+        ResultDescription = AgvAction is null ? ResultDescription : AgvAction.ResultDescription;
+    }
+
+    protected override Task ExecuteAction()
+    {
+        return base.ExecuteAction();
+    }
+}

RobotApp/Services/Robot/RobotActionStorage.cs

@@ -18,6 +18,8 @@ public class RobotActionStorage(IServiceProvider ServiceProvider)
         ActionType.cancelOrder => new RobotCancelOrderAction(ServiceProvider),
         ActionType.liftUp => new RobotLiftUpAction(ServiceProvider),
         ActionType.liftDown => new RobotLiftDownAction(ServiceProvider),
+        ActionType.drop => new RobotDropAction(ServiceProvider),
+        ActionType.pick => new RobotPickAction(ServiceProvider),
         ActionType.liftRotate => new RobotLiftRotateAction(ServiceProvider),
         ActionType.rotate => new RobotRotateAction(ServiceProvider),
         ActionType.rotateKeepLift => new RobotRotateKeepLift(ServiceProvider),

RobotApp/Services/Robot/RobotController.cs

@@ -40,13 +40,13 @@ public partial class RobotController(IOrder OrderManager,
             try
             {
                 if (StateManager.RootStateName != RootStateType.Auto.ToString()) throw new OrderException(RobotErrors.Error1013(StateManager.RootStateName));
-                if (string.IsNullOrEmpty(OrderManager.OrderId))
+                //if (OrderManager.NodeStates.Length > 0)
-                {
+                //{
-                    if (ActionManager.HasActionRunning) throw new OrderException(RobotErrors.Error1007());
+                //    if (ActionManager.HasActionRunning) throw new OrderException(RobotErrors.Error1007());
-                    if (ErrorManager.HasFatalError) throw new OrderException(RobotErrors.Error1008());
+                //    if (ErrorManager.HasFatalError) throw new OrderException(RobotErrors.Error1008());
-                    if (NavigationManager.Driving) throw new OrderException(RobotErrors.Error1009());
+                //    if (NavigationManager.Driving) throw new OrderException(RobotErrors.Error1009());
-                }
+                //}
-                else if (order.OrderId != OrderManager.OrderId) throw new OrderException(RobotErrors.Error1001(OrderManager.OrderId, order.OrderId));
+                //else if (order.OrderId != OrderManager.OrderId) throw new OrderException(RobotErrors.Error1001(OrderManager.OrderId, order.OrderId));
                 OrderManager.UpdateOrder(order);
             }
             catch (RobotExeption orEx)

RobotApp/Services/Robot/RobotControllerInitialize.cs

@@ -54,6 +54,8 @@ public partial class RobotController
                 }
             }
             else StateManager.TransitionTo(RootStateType.Auto);
+
+            ErrorManager.AddError(RobotErrors.Error2001());
         }
         catch (Exception ex)
         {

RobotApp/Services/Robot/RobotFactsheet.cs

@@ -22,6 +22,8 @@ public class RobotFactsheet(RobotConnection RobotConnection, RobotConfiguration
             { ActionType.cancelOrder, CancelOrder},
             { ActionType.liftUp, LiftUp},
             { ActionType.liftDown, LiftDown},
+            { ActionType.pick, Pick},
+            { ActionType.drop, Drop},
             { ActionType.liftRotate, LiftRotate},
             { ActionType.rotate, Rotate},
             { ActionType.rotateKeepLift, RotateKeepLift},
@@ -185,6 +187,26 @@ public class RobotFactsheet(RobotConnection RobotConnection, RobotConfiguration
         BlockingTypes = [BlockingType.HARD.ToString()],
     };
 
+    public readonly static AgvAction Pick = new()
+    {
+        ActionType = ActionType.pick.ToString(),
+        ActionDescription = "Nâng cao pallet.",
+        ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()],
+        ActionParameters = [],
+        ResultDescription = "Robot đã nâng cao pallet.",
+        BlockingTypes = [BlockingType.HARD.ToString()],
+    };
+
+    public readonly static AgvAction Drop = new()
+    {
+        ActionType = ActionType.drop.ToString(),
+        ActionDescription = "Hạ thấp pallet.",
+        ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()],
+        ActionParameters = [],
+        ResultDescription = "Robot đã hạ thấpo pallet.",
+        BlockingTypes = [BlockingType.HARD.ToString()],
+    };
+
     public readonly static AgvAction LiftRotate = new()
     {
         ActionType = ActionType.liftRotate.ToString(),

RobotApp/Services/Robot/RobotInfomations.cs

@@ -1,7 +1,5 @@
 using RobotApp.Interfaces;
-using RobotApp.Services.State;
 using RobotApp.VDA5050.State;
-using RobotApp.VDA5050.Type;
 
 namespace RobotApp.Services.Robot;
 

RobotApp/Services/Robot/RobotLoads.cs

@@ -5,7 +5,8 @@ namespace RobotApp.Services.Robot;
 
 public class RobotLoads(IPeripheral PeriperalManager) : ILoad
 {
-    public Load[] Load => PeriperalManager.HasLoad ? [GetLoad()] : [];
+    //public Load[] Load => PeriperalManager.HasLoad ? [GetLoad()] : [];
+    public Load[] Load { get; private set; } = [];
     private static Load GetLoad()
     {
         return new()
@@ -28,4 +29,14 @@ public class RobotLoads(IPeripheral PeriperalManager) : ILoad
             Weight = 999
         };
     }
+
+    public void AddLoad()
+    {
+        Load = [new()];
+    }
+
+    public void ClearLoad()
+    {
+        Load = [];
+    }
 }

RobotApp/Services/Robot/RobotLocalization.cs

@@ -36,7 +36,7 @@ public class RobotLocalization(RobotConfiguration RobotConfiguration, Simulation
     public string SlamStateDetail => IsSimulation ? "" : XlocData.SlamStateDetail;
     public string CurrentActiveMap => IsSimulation ? "" : XlocData.CurrentActiveMap;
     public double Reliability => IsSimulation ? 100 : XlocData.Reliability;
-    public double MatchingScore => IsSimulation ? 100 : XlocData.MatchingScore;
+    public double MatchingScore => IsSimulation ? 1.0 : XlocData.MatchingScore;
     public bool IsReady => IsSimulation || XlocData.IsReady;
 
     private readonly XlocData XlocData = new();

RobotApp/Services/Robot/RobotOrderController.cs

@@ -1,14 +1,13 @@
-using RobotApp.Common.Shares.Enums;
+using MudBlazor;
+using RobotApp.Common.Shares.Enums;
 using RobotApp.Interfaces;
 using RobotApp.Services.Exceptions;
-using RobotApp.Services.Robot.Actions;
 using RobotApp.Services.State;
 using RobotApp.VDA5050.InstantAction;
 using RobotApp.VDA5050.Order;
 using RobotApp.VDA5050.State;
 using System.Collections.Concurrent;
 using System.Data;
-using System.Threading.Tasks;
 using Action = RobotApp.VDA5050.InstantAction.Action;
 
 namespace RobotApp.Services.Robot;
@@ -34,10 +33,11 @@ public class RobotOrderController(INavigation NavigationManager,
 
     private readonly Dictionary<string, Action[]> OrderActions = [];
     private readonly ConcurrentQueue<Action> ActionWaitingRunning = [];
+    private List<Action> FinalAction = [];
 
     private OrderMsg? NewOrder;
     private Node[] Nodes = [];
-    private Edge[] Edges = [];
+    private VDA5050.Order.Edge[] Edges = [];
     private Node? CurrentBaseNode;
     private Node? LastNode;
 
@@ -46,6 +46,8 @@ public class RobotOrderController(INavigation NavigationManager,
     private bool IsCancelOrder = false;
     private bool IsActionRunning = false;
     private bool IsWaitingPaused = false;
+    private bool IsNavigationFinished = false;
+    private bool IsOneceNode = false;
     private Action? ActionHard = null;
 
     public void UpdateOrder(OrderMsg order)
@@ -59,7 +61,7 @@ public class RobotOrderController(INavigation NavigationManager,
 
     public void StopOrder()
     {
-        if (!string.IsNullOrEmpty(OrderId) || OrderTimer is not null) IsCancelOrder = true;
+        if (NodeStates.Length > 0 || OrderTimer is not null) IsCancelOrder = true;
     }
 
     public void PauseOrder()
@@ -82,6 +84,17 @@ public class RobotOrderController(INavigation NavigationManager,
     {
         OrderTimer?.Dispose();
         OrderTimer = null;
+        //OrderId = string.Empty;
+        //OrderUpdateId = 0;
+        OrderActions.Clear();
+        ActionWaitingRunning.Clear();
+        CurrentBaseNode = null;
+        Nodes = [];
+        Edges = [];
+        UpdateState();
+        SafetyManager.OnSafetySpeedChanged -= OnSafetySpeedChanged;
+        NavigationManager.OnNavigationFinished -= NavigationFinished;
+        StateManager.TransitionTo(AutoStateType.Idle);
     }
 
     private Node? GetCurrentNode()
@@ -91,9 +104,10 @@ public class RobotOrderController(INavigation NavigationManager,
         foreach (var node in Nodes)
         {
             var distance = Localization.DistanceTo(node.NodePosition.X, node.NodePosition.Y);
-            if (distance <= node.NodePosition.AllowedDeviationXY)
+            var nodeMin = node.NodePosition.AllowedDeviationXY == 0.0 ? 0.5 : node.NodePosition.AllowedDeviationXY;
+            if (distance <= nodeMin)
             {
-                if (distance < minDistance)
+                if (distance < minDistance && node.NodeId != Nodes[^1].NodeId)
                 {
                     minDistance = distance;
                     inNode = node;
@@ -105,17 +119,7 @@ public class RobotOrderController(INavigation NavigationManager,
 
     private void NavigationFinished()
     {
-        HandleOrderStop();
+        IsNavigationFinished = true;
-        SafetyManager.OnSafetySpeedChanged -= OnSafetySpeedChanged;
-        OrderId = string.Empty;
-        OrderUpdateId = 0;
-        OrderActions.Clear();
-        ActionWaitingRunning.Clear();
-        CurrentBaseNode = null;
-        Nodes = [];
-        Edges = [];
-        UpdateState();
-        StateManager.TransitionTo(AutoStateType.Idle);
     }
 
     private void OnSafetySpeedChanged(SafetySpeed safetySpeed)
@@ -153,10 +157,24 @@ public class RobotOrderController(INavigation NavigationManager,
 
     private void HandleNewOrder(OrderMsg order)
     {
-        if (NavigationManager.State != NavigationState.Idle) throw new OrderException(RobotErrors.Error1012(NavigationManager.State));
+        //if (NavigationManager.State != NavigationState.Idle) throw new OrderException(RobotErrors.Error1012(NavigationManager.State));
 
         OrderActions.Clear();
         LastNode = null;
+        FinalAction = [];
+        IsNavigationFinished = false;
+
+        for (int i = 0; i < order.Edges.Length; i++)
+        {
+            if (order.Edges[i].Trajectory is null)
+            {
+                order.Edges[i].Trajectory = new Trajectory()
+                {
+                    Degree = 1,
+                    ControlPoints = [new ControlPoint() { X = 0, Y = 0 }]
+                };
+            }
+        }
 
         for (int i = 0; i < order.Nodes.Length; i++)
         {
@@ -178,28 +196,34 @@ public class RobotOrderController(INavigation NavigationManager,
                 {
                     item.ActionDescription += $".On NodeId: {order.Nodes[i].NodeId}";
                 }
+
                 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 (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 (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];
         }
         SafetyManager.OnSafetySpeedChanged += OnSafetySpeedChanged;
 
+        if (OrderActions.TryGetValue(order.Nodes[^1].NodeId, out Action[]? finalactions) && finalactions is not null && finalactions.Length > 0) FinalAction = [.. finalactions];
         ActionManager.ClearInstantActions();
         if (OrderActions.Count > 0) ActionManager.AddOrderActions([.. OrderActions.Values.SelectMany(a => a)]);
 
-        NavigationManager.Move(order.Nodes, order.Edges);
+        if (order.Nodes.Length > 1 && order.Edges.Length >= 0)
-        NavigationManager.OnNavigationFinished += NavigationFinished;
+        {
+            NavigationManager.Move(order.Nodes, order.Edges);
+            NavigationManager.OnNavigationFinished += NavigationFinished;
+        }
+        else IsOneceNode = true;
         OrderId = order.OrderId;
         OrderUpdateId = order.OrderUpdateId;
         Nodes = order.Nodes;
         Edges = order.Edges;
-        if (CurrentBaseNode is not null) NavigationManager.UpdateOrder(CurrentBaseNode.NodeId);
+        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();
     }
@@ -248,6 +272,24 @@ public class RobotOrderController(INavigation NavigationManager,
             return;
         }
 
+        if (IsNavigationFinished || IsOneceNode)
+        {
+            if (FinalAction.Count > 0)
+            {
+                var action = FinalAction[0];
+                var robotAction = ActionManager[action.ActionId];
+                if (robotAction is null) return;
+                if (robotAction.IsCompleted) FinalAction.Remove(action);
+                if (robotAction.Status == ActionStatus.WAITING) ActionManager.StartOrderAction(action.ActionId);
+            }
+            else
+            {
+                LastNode = Nodes[^1]; // ddoanj nay phai sua lai kien tra xem co toi node cuoi that khong
+                HandleOrderStop();
+                return;
+            }
+        }
+
         var currentNode = GetCurrentNode();
         if (currentNode is not null && currentNode.NodeId != LastNode?.NodeId)
         {
@@ -319,10 +361,10 @@ public class RobotOrderController(INavigation NavigationManager,
                 }
 
                 if (NewOrderHandler.Nodes.Length < 2) throw new OrderException(RobotErrors.Error1002(NewOrderHandler.Nodes.Length));
-                if (NewOrderHandler.Edges.Length < 1) throw new OrderException(RobotErrors.Error1003(NewOrderHandler.Edges.Length));
+                //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 (NewOrderHandler.Edges.Length != NewOrderHandler.Nodes.Length - 1) throw new OrderException(RobotErrors.Error1004(NewOrderHandler.Nodes.Length, NewOrderHandler.Edges.Length));
 
-                if (string.IsNullOrEmpty(OrderId)) HandleNewOrder(NewOrderHandler);
+                if (NodeStates.Length == 0) HandleNewOrder(NewOrderHandler);
                 else HandleUpdateOrder(NewOrderHandler);
             }
             HandleOrder();

RobotApp/Services/Robot/RobotPathPlanner.cs

@@ -13,7 +13,7 @@ public class RobotPathPlanner(IConfiguration Configuration)
     private readonly double ReverseDirectionAngleDegree = Configuration.GetValue("PathPlanning:ReverseDirectionAngleDegree", 89);
     private readonly double Ratio = Configuration.GetValue("PathPlanning:Ratio", 0.1);
 
-    public RobotDirection GetDirectionInNode(double currentTheta,Node inNode, Node futureNode, Edge edge)
+    public RobotDirection GetDirectionInNode(double currentTheta, Node inNode, Node futureNode, Edge edge)
     {
         (double futurex, double futurey) = MathExtensions.Curve(0.1, new()
         {
@@ -26,8 +26,8 @@ public class RobotPathPlanner(IConfiguration Configuration)
             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 robotx, double roboty) = 
+        (double robotx, double roboty) =
         (
             inNode.NodePosition.X + Math.Cos(currentTheta * Math.PI / 180),
             inNode.NodePosition.Y + Math.Sin(currentTheta * Math.PI / 180)
@@ -72,7 +72,7 @@ public class RobotPathPlanner(IConfiguration Configuration)
                 ControlPoint2X = edges[i - 1].Trajectory.ControlPoints.Length > 3 ? edges[i - 1].Trajectory.ControlPoints[2].X : 0,
                 ControlPoint2Y = edges[i - 1].Trajectory.ControlPoints.Length > 3 ? edges[i - 1].Trajectory.ControlPoints[2].Y : 0,
                 TrajectoryDegree = edges[i - 1].Trajectory.Degree == 1 ? TrajectoryDegree.One : edges[i - 1].Trajectory.Degree == 2 ? TrajectoryDegree.Two : TrajectoryDegree.Three,
-            }); 
+            });
             (double futurex, double futurey) = MathExtensions.Curve(0.1, new()
             {
                 X1 = nodes[i].NodePosition.X,

RobotApp/Services/Robot/RobotStates.cs

@@ -47,12 +47,12 @@ public class RobotStates(RobotConfiguration RobotConfiguration,
             Maps = [],
             OrderId = OrderManager.OrderId,
             OrderUpdateId = OrderManager.OrderUpdateId,
-            ZoneSetId = "",
+            ZoneSetId = "9879E7A9-CF5F-4ABD-924E-08DE1C3D25FE",
-            LastNodeId = OrderManager.LastNodeId,
+            LastNodeId = string.IsNullOrEmpty(OrderManager.LastNodeId) ? "EE9959EC-B670-4D22-8BD2-08DE1C3D71FC" : OrderManager.LastNodeId,
             LastNodeSequenceId = OrderManager.LastNodeSequenceId,
-            Driving = false,
+            Driving = NavigationManager.VelocityX > 0 || NavigationManager.Omega > 0,
             Paused = false,
-            NewBaseRequest = false,
+            NewBaseRequest = true,
             DistanceSinceLastNode = 0,
             OperatingMode = PeripheralManager.PeripheralMode.ToString(),
             NodeStates = OrderManager.NodeStates,

RobotApp/Services/Robot/Simulation/Navigation/SimulationNavigation.cs

@@ -94,12 +94,15 @@ public class SimulationNavigation : INavigation, IDisposable
         MoveFuzzy = new FuzzyLogic().WithGainP(1.1);
         MovePurePursuit = new PurePursuit().WithLookheadDistance(0.35).WithPath([.. NavigationPath]);
 
-        double angleFoward = Math.Atan2(NavigationPath[1].Y - NavigationPath[0].Y, NavigationPath[1].X - NavigationPath[0].X) * 180 / Math.PI;
+        (NavigationNode node, int index) = MovePurePursuit.GetOnNode(Visualization.X, Visualization.Y);
-        double angleBacward = Math.Atan2(NavigationPath[0].Y - NavigationPath[1].Y, NavigationPath[0].X - NavigationPath[1].X) * 180 / Math.PI;
+        if(index >= NavigationPath.Length - 1) return;
+
+        double angleFoward = Math.Atan2(NavigationPath[index + 1].Y - node.Y, NavigationPath[index + 1].X - node.X) * 180 / Math.PI;
+        double angleBacward = Math.Atan2(node.Y - NavigationPath[index + 1].Y, node.X - NavigationPath[index + 1].X) * 180 / Math.PI;
         OrderNodes = nodes;
         OrderEdges = edges;
         SimulationOrderNodes = pathDirection;
-        Rotate(SimulationOrderNodes[0].Direction == RobotDirection.FORWARD ? angleFoward : angleBacward);
+        Rotate(node.Direction == RobotDirection.FORWARD ? angleFoward : angleBacward);
     }
 
     public void MoveStraight(double x, double y)