From 9ac5270885adc6cdd8de5c9a8ebb3e8705844f16 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=C4=90=C4=83ng=20Nguy=E1=BB=85n?= Date: Fri, 17 Oct 2025 09:24:45 +0700 Subject: [PATCH] update --- RobotApp/Interfaces/IError.cs | 1 + RobotApp/Interfaces/IInfomation.cs | 8 +- RobotApp/Interfaces/ILoad.cs | 8 ++ RobotApp/Services/Robot/RobotConnection.cs | 14 +++- RobotApp/Services/Robot/RobotController.cs | 18 +---- .../Robot/RobotControllerInitialize.cs | 5 +- RobotApp/Services/Robot/RobotErrors.cs | 9 ++- RobotApp/Services/Robot/RobotInfomations.cs | 29 +++++++- RobotApp/Services/Robot/RobotLoads.cs | 30 +++++++- RobotApp/Services/Robot/RobotStates.cs | 73 ++++++++++++++++++- RobotApp/Services/Robot/RobotVisualization.cs | 7 +- RobotApp/Services/State/RobotStateMachine.cs | 20 ++--- 12 files changed, 177 insertions(+), 45 deletions(-) create mode 100644 RobotApp/Interfaces/ILoad.cs diff --git a/RobotApp/Interfaces/IError.cs b/RobotApp/Interfaces/IError.cs index 09ff344..97c2a97 100644 --- a/RobotApp/Interfaces/IError.cs +++ b/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); diff --git a/RobotApp/Interfaces/IInfomation.cs b/RobotApp/Interfaces/IInfomation.cs index 8563825..a9b283d 100644 --- a/RobotApp/Interfaces/IInfomation.cs +++ b/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(); } diff --git a/RobotApp/Interfaces/ILoad.cs b/RobotApp/Interfaces/ILoad.cs new file mode 100644 index 0000000..1eeb138 --- /dev/null +++ b/RobotApp/Interfaces/ILoad.cs @@ -0,0 +1,8 @@ +using RobotApp.VDA5050.State; + +namespace RobotApp.Interfaces; + +public interface ILoad +{ + Load? Load { get; } +} diff --git a/RobotApp/Services/Robot/RobotConnection.cs b/RobotApp/Services/Robot/RobotConnection.cs index 8aca3a2..ce97d0b 100644 --- a/RobotApp/Services/Robot/RobotConnection.cs +++ b/RobotApp/Services/Robot/RobotConnection.cs @@ -6,20 +6,25 @@ using System.Text.Json; namespace RobotApp.Services.Robot; -public class RobotConnection(RobotConfiguration RobotConfiguration, Logger Logger, Logger MQTTClientLogger) +public class RobotConnection(RobotConfiguration RobotConfiguration, + Logger Logger, + Logger 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? OrderUpdated; + public event Action? ActionUpdated; + private void OrderChanged(string data) { try { var msg = JsonSerializer.Deserialize(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(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) { diff --git a/RobotApp/Services/Robot/RobotController.cs b/RobotApp/Services/Robot/RobotController.cs index 06f97ce..6a01110 100644 --- a/RobotApp/Services/Robot/RobotController.cs +++ b/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 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? 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)); diff --git a/RobotApp/Services/Robot/RobotControllerInitialize.cs b/RobotApp/Services/Robot/RobotControllerInitialize.cs index 3965f67..a8a8438 100644 --- a/RobotApp/Services/Robot/RobotControllerInitialize.cs +++ b/RobotApp/Services/Robot/RobotControllerInitialize.cs @@ -33,7 +33,9 @@ public partial class RobotController await Task.Delay(1000, cancellationToken); } 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; diff --git a/RobotApp/Services/Robot/RobotErrors.cs b/RobotApp/Services/Robot/RobotErrors.cs index 4e2f750..b6b2189 100644 --- a/RobotApp/Services/Robot/RobotErrors.cs +++ b/RobotApp/Services/Robot/RobotErrors.cs @@ -6,14 +6,17 @@ namespace RobotApp.Services.Robot; public class RobotErrors : IError { private readonly List 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() - => new(); + public static Error Error1013(string rootState) + => 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) diff --git a/RobotApp/Services/Robot/RobotInfomations.cs b/RobotApp/Services/Robot/RobotInfomations.cs index b329cff..7a3d478 100644 --- a/RobotApp/Services/Robot/RobotInfomations.cs +++ b/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 { - public class RobotInfomations : IInfomation + private readonly List 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(); + } } } diff --git a/RobotApp/Services/Robot/RobotLoads.cs b/RobotApp/Services/Robot/RobotLoads.cs index d671692..5fe7a8a 100644 --- a/RobotApp/Services/Robot/RobotLoads.cs +++ b/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 + }; + } } diff --git a/RobotApp/Services/Robot/RobotStates.cs b/RobotApp/Services/Robot/RobotStates.cs index f47bdfc..b23014c 100644 --- a/RobotApp/Services/Robot/RobotStates.cs +++ b/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 Logger, + IOrder OrderManager, + IPeripheral PeripheralManager) : BackgroundService { + private uint HeaderId = 0; + private readonly string SerialNumber = RobotConfiguration.SerialNumber; + + private WatchTimerAsync? 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); + } } diff --git a/RobotApp/Services/Robot/RobotVisualization.cs b/RobotApp/Services/Robot/RobotVisualization.cs index e741c0a..bddbc6f 100644 --- a/RobotApp/Services/Robot/RobotVisualization.cs +++ b/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 Logger) : BackgroundService { - public string SerialNumber { get; set; } = RobotConfiguration.SerialNumber; + public string SerialNumber = RobotConfiguration.SerialNumber; private uint HeaderId; private WatchTimerAsync? UpdateTimer; - private const int UpdateInterval = 50; + private const int UpdateInterval = 100; private VisualizationMsg GetVisualizationMsg() { return new VisualizationMsg() @@ -46,7 +46,8 @@ public class RobotVisualization(ILocalization Localization, INavigation Navigati protected override async Task ExecuteAsync(CancellationToken stoppingToken) { - while(!stoppingToken.IsCancellationRequested) + await Task.Yield(); + while (!stoppingToken.IsCancellationRequested) { if (RobotConnection.IsConnected) break; await Task.Delay(1000); diff --git a/RobotApp/Services/State/RobotStateMachine.cs b/RobotApp/Services/State/RobotStateMachine.cs index fdbe1bf..fd698b6 100644 --- a/RobotApp/Services/State/RobotStateMachine.cs +++ b/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 Logger) : IDisposable public event Action? 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 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"); - return; - } - lock (StateLock) - { - CurrentState?.Update(); + current = current.SuperState; } + + return current.Name.ToString(); } public string GetCurrentStatePath() { - if (CurrentState == null) return "No State"; + if (CurrentState == null) return "Unknown"; return GetStatePath(CurrentState); }