update

RobotApp/Interfaces/IError.cs

@@ -4,6 +4,7 @@ namespace RobotApp.Interfaces;
 
 public interface IError
 {
+    event Action? OnNewFatalError;
     bool HasFatalError { get; }
     void AddError(Error error, TimeSpan? clearAfter = null);
     void DeleteErrorType(string errorType);

RobotApp/Interfaces/IInfomation.cs

@@ -1,6 +1,10 @@
-namespace RobotApp.Interfaces;
+using RobotApp.VDA5050.State;
+
+namespace RobotApp.Interfaces;
 
 public interface IInfomation
 {
-
+    void AddInfo(Information infor);
+    void DeleteInfoType(string infoType);
+    void ClearAllInfos();
 }

RobotApp/Interfaces/ILoad.cs

@@ -0,0 +1,8 @@
+using RobotApp.VDA5050.State;
+
+namespace RobotApp.Interfaces;
+
+public interface ILoad
+{
+    Load? Load { get; }
+}

RobotApp/Services/Robot/RobotConnection.cs

@@ -6,20 +6,25 @@ using System.Text.Json;
 
 namespace RobotApp.Services.Robot;
 
-public class RobotConnection(RobotConfiguration RobotConfiguration, Logger<RobotConnection> Logger, Logger<MQTTClient> MQTTClientLogger)
+public class RobotConnection(RobotConfiguration RobotConfiguration, 
+                             Logger<RobotConnection> Logger,
+                             Logger<MQTTClient> MQTTClientLogger)
 {
     private readonly VDA5050Setting VDA5050Setting = RobotConfiguration.VDA5050Setting;
     private MQTTClient? MqttClient;
 
     public bool IsConnected => MqttClient is not null && MqttClient.IsConnected;
-    public readonly string SerialNumber = RobotConfiguration.SerialNumber;
+    public event Action<OrderMsg>? OrderUpdated;
+    public event Action<InstantActionsMsg>? ActionUpdated;
+
 
     private void OrderChanged(string data)
     {
         try
         {
             var msg = JsonSerializer.Deserialize<OrderMsg>(data);
-            if (msg is null || string.IsNullOrEmpty(msg.SerialNumber) || msg.SerialNumber != SerialNumber) return;
+            if (msg is null || string.IsNullOrEmpty(msg.SerialNumber) || msg.SerialNumber != RobotConfiguration.SerialNumber) return;
+            OrderUpdated?.Invoke(msg);
         }
         catch (Exception ex)
         {
@@ -32,7 +37,8 @@ public class RobotConnection(RobotConfiguration RobotConfiguration, Logger<Robot
         try
         {
             var msg = JsonSerializer.Deserialize<InstantActionsMsg>(data);
-            if (msg is null || string.IsNullOrEmpty(msg.SerialNumber) || msg.SerialNumber != SerialNumber) return;
+            if (msg is null || string.IsNullOrEmpty(msg.SerialNumber) || msg.SerialNumber != RobotConfiguration.SerialNumber) return;
+            ActionUpdated?.Invoke(msg);
         }
         catch (Exception ex)
         {

RobotApp/Services/Robot/RobotController.cs

@@ -1,4 +1,5 @@
-using RobotApp.Interfaces;
+using RobotApp.Common.Shares.Enums;
+using RobotApp.Interfaces;
 using RobotApp.Services.Exceptions;
 using RobotApp.Services.State;
 using RobotApp.VDA5050.InstantAction;
@@ -12,9 +13,7 @@ public partial class RobotController(IOrder OrderManager,
                              IBattery BatteryManager,
                              ILocalization Localization,
                              IPeripheral PeripheralManager,
-                             ISafety SafetyManager,
                              IError ErrorManager,
-                             IInfomation InfomationManager,
                              Logger<RobotController> Logger,
                              RobotConnection ConnectionManager,
                              RobotStateMachine StateManager) : BackgroundService
@@ -22,15 +21,9 @@ public partial class RobotController(IOrder OrderManager,
     private readonly Mutex NewOrderMutex = new();
     private readonly Mutex NewInstanceMutex = new();
 
-    private WatchTimer<RobotController>? UpdateStateTimer;
-    private const int UpdateStateInterval = 1000;
-
     protected override async Task ExecuteAsync(CancellationToken stoppingToken)
     {
         await InitializationingHandler(stoppingToken);
-
-        UpdateStateTimer = new(UpdateStateInterval, UpdateStateHandler, Logger);
-        UpdateStateTimer.Start();
     }
 
     public override async Task StopAsync(CancellationToken cancellationToken)
@@ -39,18 +32,13 @@ public partial class RobotController(IOrder OrderManager,
         await base.StopAsync(cancellationToken);
     }
 
-    private void UpdateStateHandler()
-    {
-        // xử lý cập nhật trạng thái robot và gửi thông tin qua kết nối
-    }
-
     public void NewOrderUpdated(OrderMsg order)
     {
         if (NewOrderMutex.WaitOne(2000))
         {
             try
             {
-                if (PeripheralManager.PeripheralMode != PeripheralMode.AUTO) throw new OrderException(RobotErrors.Error1006(PeripheralManager.PeripheralMode));
+                if (StateManager.RootStateName != RootStateType.Auto.ToString()) throw new OrderException(RobotErrors.Error1013(StateManager.RootStateName));
                 if (!string.IsNullOrEmpty(OrderManager.OrderId))
                 {
                     if (order.OrderId != OrderManager.OrderId) throw new OrderException(RobotErrors.Error1001(OrderManager.OrderId, order.OrderId));

RobotApp/Services/Robot/RobotControllerInitialize.cs

@@ -34,6 +34,8 @@ public partial class RobotController
             }
             Logger.Info("Robot đã khởi tạo xong. Đang kết nối tới Fleet Manager.");
             
+            ConnectionManager.OrderUpdated += NewOrderUpdated;
+            ConnectionManager.ActionUpdated += NewInstanceActionUpdated;
             await ConnectionManager.StartConnection(cancellationToken);
             Logger.Info("Robot đã kết nối tới Fleet Manager.");
             StateManager.TransitionTo(SystemStateType.Standby);
@@ -54,7 +56,6 @@ public partial class RobotController
 
     private void StopHandler()
     {
-        UpdateStateTimer?.Stop();
         PeripheralManager.OnPeripheralModeChanged -= SwichModeChanged;
         PeripheralManager.OnButtonPressed -= OnButtonPressed;
         PeripheralManager.OnStop -= OnStop;

RobotApp/Services/Robot/RobotErrors.cs

@@ -6,14 +6,17 @@ namespace RobotApp.Services.Robot;
 public class RobotErrors : IError
 {
     private readonly List<Error> Errors = [];
-
     public bool HasFatalError => Errors.Any(e => e.ErrorLevel == ErrorLevel.FATAL.ToString());
 
+    public event Action? OnNewFatalError;
+
     public void AddError(Error error, TimeSpan? clearAfter = null)
     {
+        if (Errors.Any(e => e.ErrorType == error.ErrorType && e.ErrorHint == error.ErrorHint)) return;
         lock (Errors)
         {
             Errors.Add(error);
+            if(error.ErrorLevel == ErrorLevel.FATAL.ToString()) OnNewFatalError?.Invoke();
         }
         if (clearAfter is not null && clearAfter.HasValue)
         {
@@ -89,8 +92,8 @@ public class RobotErrors : IError
         => CreateError(ErrorType.INITIALIZE_ORDER, "1011", ErrorLevel.WARNING, $"Order Edges không đúng thứ tự. EdgeId: {edgeId}, SequenceId: {sequenceId}, Vị trí đúng: {correctIndex}");
     public static Error Error1012(NavigationState state)
         => CreateError(ErrorType.INITIALIZE_ORDER, "1012", ErrorLevel.WARNING, $"Không thể khởi tạo order mới khi hệ thống điều hướng không ở trạng thái sẵn sàng. Trạng thái hiện tại: {state}");
-    public static Error Error1013()
+    public static Error Error1013(string rootState)
-        => new();
+        => CreateError(ErrorType.INITIALIZE_ORDER, "1013", ErrorLevel.WARNING, $"Robot chưa sẵn sàng để nhận Order. Trạng thái hiện tại {rootState}");
     public static Error Error1014(string edgeId, string nodeId)
         => CreateError(ErrorType.INITIALIZE_ORDER, "1014", ErrorLevel.WARNING, $"Edge {edgeId} chứa StartNodeId {nodeId} không tồn tại trong Nodes");
     public static Error Error1015(string edgeId, string nodeId)

RobotApp/Services/Robot/RobotInfomations.cs

@@ -1,8 +1,33 @@
 using RobotApp.Interfaces;
+using RobotApp.VDA5050.State;
+
+namespace RobotApp.Services.Robot;
 
-namespace RobotApp.Services.Robot
-{
 public class RobotInfomations : IInfomation
 {
+    private readonly List<Information> Infors = [];
+    public void AddInfo(Information infor)
+    {
+        if (Infors.Any(e => e.InfoType == infor.InfoType)) return;
+        lock (Infors)
+        {
+            Infors.Add(infor);
+        }
+    }
+
+    public void DeleteInfoType(string infoType)
+    {
+        lock (Infors)
+        {
+            Infors.RemoveAll(e => e.InfoType == infoType);
+        }
+    }
+
+    public void ClearAllInfos()
+    {
+        lock (Infors)
+        {
+            Infors.Clear();
+        }
     }
 }

RobotApp/Services/Robot/RobotLoads.cs

@@ -1,5 +1,31 @@
-namespace RobotApp.Services.Robot;
+using RobotApp.Interfaces;
+using RobotApp.VDA5050.State;
 
-public class RobotLoads 
+namespace RobotApp.Services.Robot;
+
+public class RobotLoads(IPeripheral PeriperalManager) : ILoad
 {
+    public Load? Load => PeriperalManager.HasLoad ? GetLoad() : null;
+    private static Load GetLoad()
+    {
+        return new()
+        {
+            LoadId = Guid.NewGuid().ToString(),
+            LoadDimensions = new VDA5050.Factsheet.LoadDimensions
+            {
+                Length = 0.5,
+                Width = 0.5,
+                Height = 0.5
+            },
+            LoadPosition = "on_top",
+            LoadType = "box",
+            BoundingBoxReference = new VDA5050.Factsheet.BoundingBoxReference
+            {
+                X = 0,
+                Y = 0,
+                Z = 0,
+            },
+            Weight = 999
+        };
+    }
 }

RobotApp/Services/Robot/RobotStates.cs

@@ -1,5 +1,74 @@
-namespace RobotApp.Services.Robot;
+using RobotApp.Common.Shares;
+using RobotApp.Interfaces;
+using RobotApp.VDA5050;
+using RobotApp.VDA5050.State;
+using System.Text.Json;
 
-public class RobotStates
+namespace RobotApp.Services.Robot;
+
+public class RobotStates(RobotConfiguration RobotConfiguration, RobotConnection ConnectionManager,
+                         Logger<RobotStates> Logger,
+                         IOrder OrderManager,
+                         IPeripheral PeripheralManager) : BackgroundService
 {
+    private uint HeaderId = 0;
+    private readonly string SerialNumber = RobotConfiguration.SerialNumber;
+
+    private WatchTimerAsync<RobotStates>? UpdateStateTimer;
+    private const int UpdateStateInterval = 1000;
+
+    public async Task PubState()
+    {
+        try
+        {
+            await ConnectionManager.Publish(VDA5050Topic.STATE.ToTopicString(), JsonSerializer.Serialize(GetStateMsg(), JsonOptionExtends.Write));
+        }
+        catch { }
+    }
+
+    private StateMsg GetStateMsg()
+    {
+        return new StateMsg
+        {
+            HeaderId = HeaderId++,
+            SerialNumber = SerialNumber,
+            Maps = [],
+            OrderId = OrderManager.OrderId,
+            OrderUpdateId = OrderManager.OrderUpdateId,
+            ZoneSetId = "",
+            LastNodeId = OrderManager.LastNodeId,
+            LastNodeSequenceId = OrderManager.LastNodeSequenceId,
+            Driving = false,
+            Paused = false,
+            NewBaseRequest = false,
+            DistanceSinceLastNode = 0,
+            OperatingMode = PeripheralManager.PeripheralMode.ToString(),
+        };
+    }
+
+    private async Task UpdateStateHandler()
+    {
+        await PubState();
+    }
+
+    protected override async Task ExecuteAsync(CancellationToken stoppingToken)
+    {
+        await Task.Yield();
+        while (!stoppingToken.IsCancellationRequested)
+        {
+            if (ConnectionManager.IsConnected) break;
+            await Task.Delay(1000);
+        }
+        if(!stoppingToken.IsCancellationRequested)
+        {
+            UpdateStateTimer = new(UpdateStateInterval, UpdateStateHandler, Logger);
+            UpdateStateTimer.Start();
+        }
+    }
+
+    public override Task StopAsync(CancellationToken cancellationToken)
+    {
+        UpdateStateTimer?.Dispose();
+        return base.StopAsync(cancellationToken);
+    }
 }

RobotApp/Services/Robot/RobotVisualization.cs

@@ -7,11 +7,11 @@ namespace RobotApp.Services.Robot;
 
 public class RobotVisualization(ILocalization Localization, INavigation Navigation, RobotConfiguration RobotConfiguration, RobotConnection RobotConnection, Logger<RobotVisualization> Logger) : BackgroundService
 {
-    public string SerialNumber { get; set; } = RobotConfiguration.SerialNumber;
+    public string SerialNumber = RobotConfiguration.SerialNumber;
     private uint HeaderId;
 
     private WatchTimerAsync<RobotVisualization>? UpdateTimer;
-    private const int UpdateInterval = 50;
+    private const int UpdateInterval = 100;
     private VisualizationMsg GetVisualizationMsg()
     {
         return new VisualizationMsg()
@@ -46,6 +46,7 @@ public class RobotVisualization(ILocalization Localization, INavigation Navigati
 
     protected override async Task ExecuteAsync(CancellationToken stoppingToken)
     {
+        await Task.Yield();
         while (!stoppingToken.IsCancellationRequested)
         {
             if (RobotConnection.IsConnected) break;

RobotApp/Services/State/RobotStateMachine.cs

@@ -1,5 +1,4 @@
 using RobotApp.Common.Shares.Enums;
-using RobotApp.Interfaces;
 
 namespace RobotApp.Services.State;
 
@@ -11,6 +10,7 @@ public record RobotStateMachine(Logger<RobotStateMachine> Logger) : IDisposable
     public event Action<IRobotState?, IRobotState>? OnStateChanged;
     public bool IsInitialized = false;
     public string CurrentStateName => CurrentState.Name.ToString();
+    public string RootStateName => GetRootStateName();
 
     private readonly Lock StateLock = new();
 
@@ -330,22 +330,22 @@ public record RobotStateMachine(Logger<RobotStateMachine> Logger) : IDisposable
         return false;
     }
 
-    public void Update()
+    public string GetRootStateName()
     {
-        if (!IsInitialized)
+        if (CurrentState == null) return "Unknown";
+
+        var current = CurrentState;
+        while (current.SuperState != null)
         {
-            Logger.Warning("State Machine chưa được khởi tạo");
+            current = current.SuperState;
-            return;
-        }
-        lock (StateLock)
-        {
-            CurrentState?.Update();
         }
+
+        return current.Name.ToString();
     }
 
     public string GetCurrentStatePath()
     {
-        if (CurrentState == null) return "No State";
+        if (CurrentState == null) return "Unknown";
         return GetStatePath(CurrentState);
     }