diff --git a/RobotApp.Common.Shares/MathExtensions.cs b/RobotApp.Common.Shares/MathExtensions.cs index 6122d33..2cf7d47 100644 --- a/RobotApp.Common.Shares/MathExtensions.cs +++ b/RobotApp.Common.Shares/MathExtensions.cs @@ -102,4 +102,9 @@ public static class MathExtensions if (dotProduct / (lengthAB * lengthBC) < -1) return 180; return Math.Acos(dotProduct / (lengthAB * lengthBC)) * (180.0 / Math.PI); } + + public static double Distance(double x1, double y1, double x2, double y2) + { + return Math.Sqrt(Math.Pow(x2 - x1, 2) + Math.Pow(y2 - y1, 2)); + } } diff --git a/RobotApp.VDA5050/Type/InformationType.cs b/RobotApp.VDA5050/Type/InformationType.cs new file mode 100644 index 0000000..81031a6 --- /dev/null +++ b/RobotApp.VDA5050/Type/InformationType.cs @@ -0,0 +1,12 @@ +namespace RobotApp.VDA5050.Type; + +public enum InformationType +{ + robot_general +} + + +public enum InformationReferencesKey +{ + robot_state, +} diff --git a/RobotApp/Interfaces/IError.cs b/RobotApp/Interfaces/IError.cs index 97c2a97..bfe4886 100644 --- a/RobotApp/Interfaces/IError.cs +++ b/RobotApp/Interfaces/IError.cs @@ -4,6 +4,7 @@ namespace RobotApp.Interfaces; public interface IError { + Error[] ErrorsState { get; } event Action? OnNewFatalError; bool HasFatalError { get; } void AddError(Error error, TimeSpan? clearAfter = null); diff --git a/RobotApp/Interfaces/IInfomation.cs b/RobotApp/Interfaces/IInfomation.cs index a9b283d..5382b48 100644 --- a/RobotApp/Interfaces/IInfomation.cs +++ b/RobotApp/Interfaces/IInfomation.cs @@ -4,6 +4,7 @@ namespace RobotApp.Interfaces; public interface IInfomation { + Information[] InformationState { get; } void AddInfo(Information infor); void DeleteInfoType(string infoType); void ClearAllInfos(); diff --git a/RobotApp/Interfaces/IInstantActions.cs b/RobotApp/Interfaces/IInstantActions.cs index d5ea547..d6b6362 100644 --- a/RobotApp/Interfaces/IInstantActions.cs +++ b/RobotApp/Interfaces/IInstantActions.cs @@ -8,7 +8,7 @@ public interface IInstantActions ActionState[] ActionStates { get; } bool HasActionRunning { get; } void AddOrderActions(Action[] actions); - void AddInstantAction(Action action); + void AddInstantAction(Action[] action); void StartOrderAction(string actionId, bool wait); void ClearInstantActions(); void PauseActions(); diff --git a/RobotApp/Interfaces/ILoad.cs b/RobotApp/Interfaces/ILoad.cs index 1eeb138..086f554 100644 --- a/RobotApp/Interfaces/ILoad.cs +++ b/RobotApp/Interfaces/ILoad.cs @@ -4,5 +4,5 @@ namespace RobotApp.Interfaces; public interface ILoad { - Load? Load { get; } + Load[] Load { get; } } diff --git a/RobotApp/Interfaces/ILocalization.cs b/RobotApp/Interfaces/ILocalization.cs index dc24872..ce136c5 100644 --- a/RobotApp/Interfaces/ILocalization.cs +++ b/RobotApp/Interfaces/ILocalization.cs @@ -128,4 +128,12 @@ public interface ILocalization /// /// MessageResult ResetSlamError(); + + /// + /// Khoảng cách từ vị trí hiện tại đến tọa độ (x, y) trong hệ tọa độ toàn cục. + /// + /// + /// + /// + double DistanceTo(double x, double y); } diff --git a/RobotApp/Interfaces/INavigation.cs b/RobotApp/Interfaces/INavigation.cs index 8e5ff03..278752c 100644 --- a/RobotApp/Interfaces/INavigation.cs +++ b/RobotApp/Interfaces/INavigation.cs @@ -26,6 +26,8 @@ public enum NavigationProccess public interface INavigation { + event Action? OnNavigationStateChanged; + event Action? OnNavigationFinished; bool IsReady { get; } bool Driving { get; } double VelocityX { get; } @@ -35,7 +37,7 @@ public interface INavigation void Move(Node[] nodes, Edge[] edges); void MoveStraight(double x, double y); void Rotate(double angle); - void Paused(); + void Pause(); void Resume(); void UpdateOrder(string lastBaseNodeId); void RefreshOrder(Node[] nodes, Edge[] edges); diff --git a/RobotApp/Interfaces/IOrder.cs b/RobotApp/Interfaces/IOrder.cs index 9e88935..810b08e 100644 --- a/RobotApp/Interfaces/IOrder.cs +++ b/RobotApp/Interfaces/IOrder.cs @@ -13,8 +13,7 @@ public interface IOrder NodeState[] NodeStates { get; } EdgeState[] EdgeStates { get; } - void StartOrder(string orderId, Node[] nodes, Edge[] edges); - void UpdateOrder(int orderUpdateId, Node[] nodes, Edge[] edges); + void UpdateOrder(OrderMsg order); void StopOrder(); void PauseOrder(); void ResumeOrder(); diff --git a/RobotApp/Interfaces/IPeripheral.cs b/RobotApp/Interfaces/IPeripheral.cs index 86419e2..7687446 100644 --- a/RobotApp/Interfaces/IPeripheral.cs +++ b/RobotApp/Interfaces/IPeripheral.cs @@ -4,7 +4,7 @@ namespace RobotApp.Interfaces; public enum PeripheralMode { - AUTO, + AUTOMATIC, MANUAL, SERVICE, } diff --git a/RobotApp/Services/Exceptions/ActionException.cs b/RobotApp/Services/Exceptions/ActionException.cs index 8fabf43..fdf8c8f 100644 --- a/RobotApp/Services/Exceptions/ActionException.cs +++ b/RobotApp/Services/Exceptions/ActionException.cs @@ -2,7 +2,7 @@ namespace RobotApp.Services.Exceptions; -public class ActionException : Exception +public class ActionException : RobotExeption { public ActionException(string message) : base(message) { } public ActionException(string message, Exception inner) : base(message, inner) { } @@ -11,5 +11,4 @@ public class ActionException : Exception { Error = error; } - public Error? Error { get; set; } } diff --git a/RobotApp/Services/Exceptions/ModbusException.cs b/RobotApp/Services/Exceptions/ModbusException.cs index 88ca6ff..a4dea4c 100644 --- a/RobotApp/Services/Exceptions/ModbusException.cs +++ b/RobotApp/Services/Exceptions/ModbusException.cs @@ -2,7 +2,7 @@ namespace RobotApp.Services.Exceptions; -public class ModbusException : Exception +public class ModbusException : RobotExeption { public ModbusException(string message) : base(message) { } public ModbusException(string message, Exception inner) : base(message, inner) { } @@ -11,5 +11,4 @@ public class ModbusException : Exception { Error = error; } - public Error? Error { get; set; } } diff --git a/RobotApp/Services/Exceptions/OrderException.cs b/RobotApp/Services/Exceptions/OrderException.cs index aaf044f..8a3094b 100644 --- a/RobotApp/Services/Exceptions/OrderException.cs +++ b/RobotApp/Services/Exceptions/OrderException.cs @@ -2,7 +2,7 @@ namespace RobotApp.Services.Exceptions; -public class OrderException : Exception +public class OrderException : RobotExeption { public OrderException(string message) : base(message) { } public OrderException(string message, Exception inner) : base(message, inner) { } @@ -11,5 +11,4 @@ public class OrderException : Exception { Error = error; } - public Error? Error { get; set; } } diff --git a/RobotApp/Services/Exceptions/PathPlannerException.cs b/RobotApp/Services/Exceptions/PathPlannerException.cs index 13ac794..08382f5 100644 --- a/RobotApp/Services/Exceptions/PathPlannerException.cs +++ b/RobotApp/Services/Exceptions/PathPlannerException.cs @@ -2,7 +2,7 @@ namespace RobotApp.Services.Exceptions; -public class PathPlannerException : Exception +public class PathPlannerException : RobotExeption { public PathPlannerException(string message) : base(message) { } public PathPlannerException(string message, Exception inner) : base(message, inner) { } @@ -11,5 +11,4 @@ public class PathPlannerException : Exception { Error = error; } - public Error? Error { get; set; } } diff --git a/RobotApp/Services/Exceptions/RobotExeption.cs b/RobotApp/Services/Exceptions/RobotExeption.cs new file mode 100644 index 0000000..7781df8 --- /dev/null +++ b/RobotApp/Services/Exceptions/RobotExeption.cs @@ -0,0 +1,15 @@ +using RobotApp.VDA5050.State; + +namespace RobotApp.Services.Exceptions; + +public class RobotExeption : Exception +{ + public RobotExeption(string message) : base(message) { } + public RobotExeption(string message, Exception inner) : base(message, inner) { } + public RobotExeption() : base() { } + public RobotExeption(Error error) : base() + { + Error = error; + } + public Error? Error { get; set; } +} diff --git a/RobotApp/Services/Exceptions/SimulationException.cs b/RobotApp/Services/Exceptions/SimulationException.cs index cb6366c..1b589d3 100644 --- a/RobotApp/Services/Exceptions/SimulationException.cs +++ b/RobotApp/Services/Exceptions/SimulationException.cs @@ -2,7 +2,7 @@ namespace RobotApp.Services.Exceptions; -public class SimulationException : Exception +public class SimulationException : RobotExeption { public SimulationException(string message) : base(message) { } public SimulationException(string message, Exception inner) : base(message, inner) { } @@ -11,5 +11,4 @@ public class SimulationException : Exception { Error = error; } - public Error? Error { get; set; } } diff --git a/RobotApp/Services/Robot/Actions/RobotAction.cs b/RobotApp/Services/Robot/Actions/RobotAction.cs index 0b30496..4edefce 100644 --- a/RobotApp/Services/Robot/Actions/RobotAction.cs +++ b/RobotApp/Services/Robot/Actions/RobotAction.cs @@ -1,4 +1,5 @@ using Grpc.Core; +using RobotApp.Services.Exceptions; using RobotApp.VDA5050.Factsheet; using RobotApp.VDA5050.InstantAction; using RobotApp.VDA5050.State; @@ -34,7 +35,11 @@ public abstract class RobotAction(IServiceProvider serviceProvider) : IAsyncDisp Status = ActionStatus.WAITING; ActionScope = actionScope; Action = action; - return Initialize(); + Initialize(); + Id = Action.ActionId; + Description = Action.ActionDescription; + Status = ActionStatus.WAITING; + return true; } public void Start() @@ -88,7 +93,7 @@ public abstract class RobotAction(IServiceProvider serviceProvider) : IAsyncDisp { return Task.CompletedTask; } - + protected virtual Task PauseAction() { return Task.CompletedTask; @@ -99,25 +104,22 @@ public abstract class RobotAction(IServiceProvider serviceProvider) : IAsyncDisp return Task.CompletedTask; } - protected virtual bool Initialize() + protected virtual void Initialize() { - if (Action is null) return false; + if (Action is null) throw new ActionException("Khởi tạo Action không tồn tại"); Scope ??= ServiceProvider.CreateScope(); var FactsheetService = Scope.ServiceProvider.GetRequiredService(); if (Enum.TryParse(Action.ActionType, out ActionType type)) Type = type; - else return false; + else throw new ActionException($"ActionType {Action.ActionType} không hợp lệ."); - var actionStore = FactsheetService.GetAction(Type); - if (actionStore is null) return false; + var actionStore = FactsheetService.GetAction(Type) ?? throw new ActionException($"ActionType {Action.ActionType} không được thiết kế."); if (Enum.TryParse(Action.BlockingType, out BlockingType blockingType)) BlockingType = blockingType; - else return false; + else throw new ActionException($"BlockingType {Action.BlockingType} không hợp lệ."); - if (!actionStore.BlockingTypes.Any(bt => bt == BlockingType.ToString()) || !actionStore.ActionScopes.Any(sp => sp == ActionScope.ToString())) - { - return false; - } + if (!actionStore.BlockingTypes.Any(bt => bt == BlockingType.ToString())) throw new ActionException($"BlockingType {BlockingType} không được hỗ trợ cho action {Type}."); + if (!actionStore.ActionScopes.Any(sp => sp == ActionScope.ToString())) throw new ActionException($"ActionScope {ActionScope} không được hỗ trợ cho action {Type}."); if (actionStore.ActionParameters.Length > 0) { @@ -125,20 +127,12 @@ public abstract class RobotAction(IServiceProvider serviceProvider) : IAsyncDisp { if (!parameterStore.IsOptional) { - if (!Action.ActionParameters.Any(a => a.Key == parameterStore.Key)) - { - return false; - } + if (!Action.ActionParameters.Any(a => a.Key == parameterStore.Key)) throw new ActionException($"Thiếu tham số bắt buộc '{parameterStore.Key}' cho action {Type}."); } } Parameters = Action.ActionParameters; } - - Id = Action.ActionId; - Description = Action.ActionDescription; AgvAction = actionStore; - Status = ActionStatus.WAITING; - return true; } private async Task ActionHandler() @@ -180,9 +174,10 @@ public abstract class RobotAction(IServiceProvider serviceProvider) : IAsyncDisp private Task Dispose() { ActionTimer?.Dispose(); + ActionTimer = null; return Task.CompletedTask; } - + public async ValueTask DisposeAsync() { await Dispose(); diff --git a/RobotApp/Services/Robot/Actions/RobotCancelOrderAction.cs b/RobotApp/Services/Robot/Actions/RobotCancelOrderAction.cs index dd4ad5e..c70245b 100644 --- a/RobotApp/Services/Robot/Actions/RobotCancelOrderAction.cs +++ b/RobotApp/Services/Robot/Actions/RobotCancelOrderAction.cs @@ -1,9 +1,15 @@ -namespace RobotApp.Services.Robot.Actions; +using RobotApp.Interfaces; + +namespace RobotApp.Services.Robot.Actions; public class RobotCancelOrderAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) { protected override Task StartAction() { + Scope ??= ServiceProvider.CreateAsyncScope(); + var RobotOrder = Scope.ServiceProvider.GetRequiredService(); + RobotOrder.StopOrder(); + Status = VDA5050.State.ActionStatus.FINISHED; return base.StartAction(); } diff --git a/RobotApp/Services/Robot/Actions/RobotFactsheetRequestAction.cs b/RobotApp/Services/Robot/Actions/RobotFactsheetRequestAction.cs index c047144..84ed488 100644 --- a/RobotApp/Services/Robot/Actions/RobotFactsheetRequestAction.cs +++ b/RobotApp/Services/Robot/Actions/RobotFactsheetRequestAction.cs @@ -1,10 +1,15 @@ -namespace RobotApp.Services.Robot.Actions; +using RobotApp.Interfaces; + +namespace RobotApp.Services.Robot.Actions; public class RobotFactsheetRequestAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) { - protected override Task StartAction() + protected override async Task StartAction() { - return base.StartAction(); + Scope ??= ServiceProvider.CreateAsyncScope(); + var RobotFactsheet = Scope.ServiceProvider.GetRequiredService(); + await RobotFactsheet.PubFactsheet(); + Status = VDA5050.State.ActionStatus.FINISHED; } protected override Task ExecuteAction() diff --git a/RobotApp/Services/Robot/Actions/RobotInitPositionAction.cs b/RobotApp/Services/Robot/Actions/RobotInitPositionAction.cs index 10ec054..841a457 100644 --- a/RobotApp/Services/Robot/Actions/RobotInitPositionAction.cs +++ b/RobotApp/Services/Robot/Actions/RobotInitPositionAction.cs @@ -1,9 +1,29 @@ -namespace RobotApp.Services.Robot.Actions; +using RobotApp.Interfaces; +using RobotApp.Services.Exceptions; + +namespace RobotApp.Services.Robot.Actions; public class RobotInitPositionAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) { + double X = 0; + double Y = 0; + double Theta = 0; + protected override Task StartAction() { + Scope ??= ServiceProvider.CreateAsyncScope(); + var Localization = Scope.ServiceProvider.GetRequiredService(); + var initPose = Localization.SetInitializePosition(X, Y, Theta); + if(initPose.IsSuccess) + { + Status = VDA5050.State.ActionStatus.FINISHED; + ResultDescription = AgvAction is null ? ResultDescription : AgvAction.ResultDescription; + } + else + { + Status = VDA5050.State.ActionStatus.FAILED; + ResultDescription = initPose.Message; + } return base.StartAction(); } @@ -11,4 +31,24 @@ public class RobotInitPositionAction(IServiceProvider ServiceProvider) : RobotAc { return base.ExecuteAction(); } + + protected override void Initialize() + { + base.Initialize(); + + var xPara = Parameters.FirstOrDefault(p => p.Key == "x") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'x'"); + var yPara = Parameters.FirstOrDefault(p => p.Key == "y") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'y'"); + var thetaPara = Parameters.FirstOrDefault(p => p.Key == "theta") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'theta'"); + + var xParse = double.TryParse(xPara.Value, out double xData); + var yParse = double.TryParse(xPara.Value, out double yData); + var thetaParse = double.TryParse(xPara.Value, out double thetaData); + + if (!xParse) throw new ActionException($"Action {Type} có parameter 'x' không đúng kiểu dữ liệu"); + if (!yParse) throw new ActionException($"Action {Type} có parameter 'y' không đúng kiểu dữ liệu"); + if (!thetaParse) throw new ActionException($"Action {Type} có parameter 'theta' không đúng kiểu dữ liệu"); + X = xData; + Y = yData; + Theta = thetaData; + } } diff --git a/RobotApp/Services/Robot/Actions/RobotLiftDownAction.cs b/RobotApp/Services/Robot/Actions/RobotLiftDownAction.cs index e142a41..b5eaddc 100644 --- a/RobotApp/Services/Robot/Actions/RobotLiftDownAction.cs +++ b/RobotApp/Services/Robot/Actions/RobotLiftDownAction.cs @@ -4,6 +4,8 @@ public class RobotLiftDownAction(IServiceProvider ServiceProvider) : RobotAction { protected override Task StartAction() { + Status = VDA5050.State.ActionStatus.FINISHED; + ResultDescription = AgvAction is null ? ResultDescription : AgvAction.ResultDescription; return base.StartAction(); } diff --git a/RobotApp/Services/Robot/Actions/RobotLiftRotateAction.cs b/RobotApp/Services/Robot/Actions/RobotLiftRotateAction.cs index 2b4e480..e730b3d 100644 --- a/RobotApp/Services/Robot/Actions/RobotLiftRotateAction.cs +++ b/RobotApp/Services/Robot/Actions/RobotLiftRotateAction.cs @@ -4,6 +4,8 @@ public class RobotLiftRotateAction(IServiceProvider ServiceProvider) : RobotActi { protected override Task StartAction() { + Status = VDA5050.State.ActionStatus.FINISHED; + ResultDescription = AgvAction is null ? ResultDescription : AgvAction.ResultDescription; return base.StartAction(); } diff --git a/RobotApp/Services/Robot/Actions/RobotLiftUpAction.cs b/RobotApp/Services/Robot/Actions/RobotLiftUpAction.cs index 6e4e796..e261142 100644 --- a/RobotApp/Services/Robot/Actions/RobotLiftUpAction.cs +++ b/RobotApp/Services/Robot/Actions/RobotLiftUpAction.cs @@ -4,6 +4,8 @@ public class RobotLiftUpAction(IServiceProvider ServiceProvider) : RobotAction(S { protected override Task StartAction() { + Status = VDA5050.State.ActionStatus.FINISHED; + ResultDescription = AgvAction is null ? ResultDescription : AgvAction.ResultDescription; return base.StartAction(); } diff --git a/RobotApp/Services/Robot/Actions/RobotMutedBaseOffAction.cs b/RobotApp/Services/Robot/Actions/RobotMutedBaseOffAction.cs index 871ee29..b86bf8f 100644 --- a/RobotApp/Services/Robot/Actions/RobotMutedBaseOffAction.cs +++ b/RobotApp/Services/Robot/Actions/RobotMutedBaseOffAction.cs @@ -1,9 +1,14 @@ -namespace RobotApp.Services.Robot.Actions; +using RobotApp.Interfaces; + +namespace RobotApp.Services.Robot.Actions; public class RobotMutedBaseOffAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) { protected override Task StartAction() { + Scope ??= ServiceProvider.CreateAsyncScope(); + var RobotSafety = Scope.ServiceProvider.GetRequiredService(); + RobotSafety.SetMutedBase(false); return base.StartAction(); } diff --git a/RobotApp/Services/Robot/Actions/RobotMutedBaseOnAction.cs b/RobotApp/Services/Robot/Actions/RobotMutedBaseOnAction.cs index e24b9f1..0e4925d 100644 --- a/RobotApp/Services/Robot/Actions/RobotMutedBaseOnAction.cs +++ b/RobotApp/Services/Robot/Actions/RobotMutedBaseOnAction.cs @@ -1,9 +1,14 @@ -namespace RobotApp.Services.Robot.Actions; +using RobotApp.Interfaces; + +namespace RobotApp.Services.Robot.Actions; public class RobotMutedBaseOnAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) { protected override Task StartAction() { + Scope ??= ServiceProvider.CreateAsyncScope(); + var RobotSafety = Scope.ServiceProvider.GetRequiredService(); + RobotSafety.SetMutedBase(true); return base.StartAction(); } diff --git a/RobotApp/Services/Robot/Actions/RobotMutedLoadOffAction.cs b/RobotApp/Services/Robot/Actions/RobotMutedLoadOffAction.cs index 4726427..caed90a 100644 --- a/RobotApp/Services/Robot/Actions/RobotMutedLoadOffAction.cs +++ b/RobotApp/Services/Robot/Actions/RobotMutedLoadOffAction.cs @@ -1,9 +1,14 @@ -namespace RobotApp.Services.Robot.Actions; +using RobotApp.Interfaces; + +namespace RobotApp.Services.Robot.Actions; public class RobotMutedLoadOffAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) { protected override Task StartAction() { + Scope ??= ServiceProvider.CreateAsyncScope(); + var RobotSafety = Scope.ServiceProvider.GetRequiredService(); + RobotSafety.SetMutedLoad(false); return base.StartAction(); } diff --git a/RobotApp/Services/Robot/Actions/RobotMutedLoadOnAction.cs b/RobotApp/Services/Robot/Actions/RobotMutedLoadOnAction.cs index 82b2c8b..fcbde1b 100644 --- a/RobotApp/Services/Robot/Actions/RobotMutedLoadOnAction.cs +++ b/RobotApp/Services/Robot/Actions/RobotMutedLoadOnAction.cs @@ -1,9 +1,14 @@ -namespace RobotApp.Services.Robot.Actions; +using RobotApp.Interfaces; + +namespace RobotApp.Services.Robot.Actions; public class RobotMutedLoadOnAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) { protected override Task StartAction() { + Scope ??= ServiceProvider.CreateAsyncScope(); + var RobotSafety = Scope.ServiceProvider.GetRequiredService(); + RobotSafety.SetMutedLoad(true); return base.StartAction(); } diff --git a/RobotApp/Services/Robot/Actions/RobotRotateAction.cs b/RobotApp/Services/Robot/Actions/RobotRotateAction.cs index ca9bce8..18a1999 100644 --- a/RobotApp/Services/Robot/Actions/RobotRotateAction.cs +++ b/RobotApp/Services/Robot/Actions/RobotRotateAction.cs @@ -1,14 +1,44 @@ -namespace RobotApp.Services.Robot.Actions; +using RobotApp.Interfaces; +using RobotApp.Services.Exceptions; + +namespace RobotApp.Services.Robot.Actions; public class RobotRotateAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) { + double Angle = 0; + INavigation? RobotNavigation; protected override Task StartAction() { + Scope ??= ServiceProvider.CreateScope(); + RobotNavigation = Scope.ServiceProvider.GetRequiredService(); + RobotNavigation.Rotate(Angle); return base.StartAction(); } protected override Task ExecuteAction() { + if(RobotNavigation is null) + { + Status = VDA5050.State.ActionStatus.FAILED; + ResultDescription = "Không tìm thấy module quản lí Navigation"; + } + else if(RobotNavigation.State == NavigationState.Idle) + { + Status = VDA5050.State.ActionStatus.FINISHED; + ResultDescription = AgvAction is null ? ResultDescription : AgvAction.ResultDescription; + } return base.ExecuteAction(); } + + protected override void Initialize() + { + base.Initialize(); + + var anglePara = Parameters.FirstOrDefault(p => p.Key == "angle") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'angle'"); + + var angleParse = double.TryParse(anglePara.Value, out double angleData); + + if (!angleParse) throw new ActionException($"Action {Type} có parameter 'angle' không đúng kiểu dữ liệu"); + Angle = angleData; + } } diff --git a/RobotApp/Services/Robot/Actions/RobotRotateKeepLift.cs b/RobotApp/Services/Robot/Actions/RobotRotateKeepLift.cs index b93d8ec..c285ee3 100644 --- a/RobotApp/Services/Robot/Actions/RobotRotateKeepLift.cs +++ b/RobotApp/Services/Robot/Actions/RobotRotateKeepLift.cs @@ -1,9 +1,14 @@ -namespace RobotApp.Services.Robot.Actions; +using RobotApp.Services.Exceptions; + +namespace RobotApp.Services.Robot.Actions; public class RobotRotateKeepLift(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) { + double Angle = 0; protected override Task StartAction() { + Status = VDA5050.State.ActionStatus.FINISHED; + ResultDescription = AgvAction is null ? ResultDescription : AgvAction.ResultDescription; return base.StartAction(); } @@ -11,4 +16,16 @@ public class RobotRotateKeepLift(IServiceProvider ServiceProvider) : RobotAction { return base.ExecuteAction(); } + + protected override void Initialize() + { + base.Initialize(); + + var anglePara = Parameters.FirstOrDefault(p => p.Key == "angle") ?? throw new ActionException($"Action {Type} không tìm thấy parameter key 'angle'"); + + var angleParse = double.TryParse(anglePara.Value, out double angleData); + + if (!angleParse) throw new ActionException($"Action {Type} có parameter 'angle' không đúng kiểu dữ liệu"); + Angle = angleData; + } } diff --git a/RobotApp/Services/Robot/Actions/RobotStartChargingAction.cs b/RobotApp/Services/Robot/Actions/RobotStartChargingAction.cs index 8b054d1..c9e3622 100644 --- a/RobotApp/Services/Robot/Actions/RobotStartChargingAction.cs +++ b/RobotApp/Services/Robot/Actions/RobotStartChargingAction.cs @@ -4,6 +4,8 @@ public class RobotStartChargingAction(IServiceProvider ServiceProvider) : RobotA { protected override Task StartAction() { + Status = VDA5050.State.ActionStatus.FINISHED; + ResultDescription = AgvAction is null ? ResultDescription : AgvAction.ResultDescription; return base.StartAction(); } diff --git a/RobotApp/Services/Robot/Actions/RobotStartPauseAction.cs b/RobotApp/Services/Robot/Actions/RobotStartPauseAction.cs index 1dded4f..80b07b2 100644 --- a/RobotApp/Services/Robot/Actions/RobotStartPauseAction.cs +++ b/RobotApp/Services/Robot/Actions/RobotStartPauseAction.cs @@ -7,8 +7,8 @@ public class RobotStartPauseAction(IServiceProvider ServiceProvider) : RobotActi protected override Task StartAction() { Scope ??= ServiceProvider.CreateScope(); - var robotController = Scope.ServiceProvider.GetRequiredService(); - robotController.Pause(); + var RobotController = Scope.ServiceProvider.GetRequiredService(); + RobotController.Pause(); Status = ActionStatus.FINISHED; ResultDescription = AgvAction is not null ? AgvAction.ResultDescription : ResultDescription; return base.StartAction(); diff --git a/RobotApp/Services/Robot/Actions/RobotStateRequestAction.cs b/RobotApp/Services/Robot/Actions/RobotStateRequestAction.cs index b36f8c7..0afbef1 100644 --- a/RobotApp/Services/Robot/Actions/RobotStateRequestAction.cs +++ b/RobotApp/Services/Robot/Actions/RobotStateRequestAction.cs @@ -2,9 +2,12 @@ public class RobotStateRequestAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) { - protected override Task StartAction() + protected override async Task StartAction() { - return base.StartAction(); + Scope ??= ServiceProvider.CreateAsyncScope(); + var RobotStates = Scope.ServiceProvider.GetRequiredService(); + await RobotStates.PubState(); + Status = VDA5050.State.ActionStatus.FINISHED; } protected override Task ExecuteAction() diff --git a/RobotApp/Services/Robot/Actions/RobotStopChargingAction.cs b/RobotApp/Services/Robot/Actions/RobotStopChargingAction.cs index 0f07d13..23a664c 100644 --- a/RobotApp/Services/Robot/Actions/RobotStopChargingAction.cs +++ b/RobotApp/Services/Robot/Actions/RobotStopChargingAction.cs @@ -4,6 +4,8 @@ public class RobotStopChargingAction(IServiceProvider ServiceProvider) : RobotAc { protected override Task StartAction() { + Status = VDA5050.State.ActionStatus.FINISHED; + ResultDescription = AgvAction is null ? ResultDescription : AgvAction.ResultDescription; return base.StartAction(); } diff --git a/RobotApp/Services/Robot/Actions/RobotStopPauseAction.cs b/RobotApp/Services/Robot/Actions/RobotStopPauseAction.cs index 49f1e0c..54de3e9 100644 --- a/RobotApp/Services/Robot/Actions/RobotStopPauseAction.cs +++ b/RobotApp/Services/Robot/Actions/RobotStopPauseAction.cs @@ -1,9 +1,16 @@ -namespace RobotApp.Services.Robot.Actions; +using RobotApp.VDA5050.State; + +namespace RobotApp.Services.Robot.Actions; public class RobotStopPauseAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) { protected override Task StartAction() { + Scope ??= ServiceProvider.CreateScope(); + var RobotController = Scope.ServiceProvider.GetRequiredService(); + RobotController.Resume(); + Status = ActionStatus.FINISHED; + ResultDescription = AgvAction is not null ? AgvAction.ResultDescription : ResultDescription; return base.StartAction(); } diff --git a/RobotApp/Services/Robot/RobotActionController.cs b/RobotApp/Services/Robot/RobotActionController.cs index e9cb321..2e8c21a 100644 --- a/RobotApp/Services/Robot/RobotActionController.cs +++ b/RobotApp/Services/Robot/RobotActionController.cs @@ -1,4 +1,5 @@ using RobotApp.Interfaces; +using RobotApp.Services.Exceptions; using RobotApp.Services.Robot.Actions; using RobotApp.VDA5050.Factsheet; using RobotApp.VDA5050.State; @@ -7,7 +8,7 @@ using System.Collections.Concurrent; namespace RobotApp.Services.Robot; -public class RobotActionController(Logger Logger, RobotActionStorage ActionStorage) : BackgroundService, IInstantActions +public class RobotActionController(Logger Logger, RobotActionStorage ActionStorage, IError ErrorManager) : BackgroundService, IInstantActions { public ActionState[] ActionStates => [.. Actions.Values.Select(a => new ActionState { @@ -25,9 +26,12 @@ public class RobotActionController(Logger Logger, RobotAc private WatchTimer? HandlerTimer; private const int HandlerInterval = 200; - public void AddInstantAction(VDA5050.InstantAction.Action action) + public void AddInstantAction(VDA5050.InstantAction.Action[] actions) { - ActionQueue.Enqueue((ActionScopes.INSTANT, action)); + foreach (var action in actions) + { + ActionQueue.Enqueue((ActionScopes.INSTANT, action)); + } } public void AddOrderActions(VDA5050.InstantAction.Action[] actions) @@ -91,9 +95,18 @@ public class RobotActionController(Logger Logger, RobotAc } } } + catch (RobotExeption orEx) + { + if (orEx.Error is not null) + { + ErrorManager.AddError(orEx.Error, TimeSpan.FromSeconds(10)); + Logger.Warning($"Lỗi khi xử lí Action: {orEx.Error.ErrorDescription}"); + } + else Logger.Warning($"Lỗi khi xử lí Action: {orEx.Message}"); + } catch (Exception ex) { - Logger?.Error($"Khởi tạo action xảy ra lỗi: {ex.Message}"); + Logger?.Error($"Lỗi khi xử lí Action: {ex.Message}"); } } @@ -113,6 +126,7 @@ public class RobotActionController(Logger Logger, RobotAc public override Task StopAsync(CancellationToken cancellationToken) { HandlerTimer?.Dispose(); + HandlerTimer = null; return base.StopAsync(cancellationToken); } } diff --git a/RobotApp/Services/Robot/RobotBattery.cs b/RobotApp/Services/Robot/RobotBattery.cs index c7c302a..c7c092b 100644 --- a/RobotApp/Services/Robot/RobotBattery.cs +++ b/RobotApp/Services/Robot/RobotBattery.cs @@ -2,35 +2,35 @@ namespace RobotApp.Services.Robot; -public class RobotBattery : IBattery +public class RobotBattery(RobotConfiguration RobotConfiguration) : IBattery { - public bool IsReady { get; private set; } = true; + public bool IsReady { get; private set; } = RobotConfiguration.IsSimulation; - public double Voltage => throw new NotImplementedException(); + public double Voltage { get; private set; } - public double Current => throw new NotImplementedException(); + public double Current { get; private set; } - public double SOC => throw new NotImplementedException(); + public double SOC { get; private set; } - public double SOH => throw new NotImplementedException(); + public double SOH { get; private set; } - public BatteryStatus Status => throw new NotImplementedException(); + public BatteryStatus Status { get; private set; } - public int ChargeTime => throw new NotImplementedException(); + public int ChargeTime { get; private set; } - public int DischargeTime => throw new NotImplementedException(); + public int DischargeTime { get; private set; } - public double Temperature => throw new NotImplementedException(); + public double Temperature { get; private set; } - public double RemainingCapacity => throw new NotImplementedException(); + public double RemainingCapacity { get; private set; } - public double RemainingEnergy => throw new NotImplementedException(); + public double RemainingEnergy { get; private set; } - public int DischargeCycles => throw new NotImplementedException(); + public int DischargeCycles { get; private set; } - public int ChargeCycles => throw new NotImplementedException(); + public int ChargeCycles { get; private set; } - public bool IsCharging => throw new NotImplementedException(); + public bool IsCharging { get; private set; } - public string[] ErrorStatus => throw new NotImplementedException(); + public string[] ErrorStatus { get; private set; } = []; } diff --git a/RobotApp/Services/Robot/RobotConfiguration.cs b/RobotApp/Services/Robot/RobotConfiguration.cs index 659af94..2854871 100644 --- a/RobotApp/Services/Robot/RobotConfiguration.cs +++ b/RobotApp/Services/Robot/RobotConfiguration.cs @@ -23,5 +23,5 @@ public class RobotConfiguration public bool IsSimulation { get; set; } = true; public SimulationModel SimulationModel { get; set; } = new(); - public string XlocAddress { get; set; } = ""; + public string XlocAddress { get; set; } = "http://192.168.195.56:50050"; } diff --git a/RobotApp/Services/Robot/RobotConnection.cs b/RobotApp/Services/Robot/RobotConnection.cs index bede774..4f93a52 100644 --- a/RobotApp/Services/Robot/RobotConnection.cs +++ b/RobotApp/Services/Robot/RobotConnection.cs @@ -22,7 +22,7 @@ public class RobotConnection(RobotConfiguration RobotConfiguration, { try { - var msg = JsonSerializer.Deserialize(data); + var msg = JsonSerializer.Deserialize(data, JsonOptionExtends.Read); if (msg is null || string.IsNullOrEmpty(msg.SerialNumber) || msg.SerialNumber != RobotConfiguration.SerialNumber) return; OrderUpdated?.Invoke(msg); } @@ -36,7 +36,7 @@ public class RobotConnection(RobotConfiguration RobotConfiguration, { try { - var msg = JsonSerializer.Deserialize(data); + var msg = JsonSerializer.Deserialize(data, JsonOptionExtends.Read); if (msg is null || string.IsNullOrEmpty(msg.SerialNumber) || msg.SerialNumber != RobotConfiguration.SerialNumber) return; ActionUpdated?.Invoke(msg); } diff --git a/RobotApp/Services/Robot/RobotController.cs b/RobotApp/Services/Robot/RobotController.cs index 69256fd..376cca1 100644 --- a/RobotApp/Services/Robot/RobotController.cs +++ b/RobotApp/Services/Robot/RobotController.cs @@ -16,7 +16,8 @@ public partial class RobotController(IOrder OrderManager, IError ErrorManager, Logger Logger, RobotConnection ConnectionManager, - RobotStateMachine StateManager) : BackgroundService + RobotStateMachine StateManager, + RobotConfiguration RobotConfiguration) : BackgroundService { private readonly Mutex NewOrderMutex = new(); private readonly Mutex NewInstanceMutex = new(); @@ -39,23 +40,23 @@ 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 (string.IsNullOrEmpty(OrderManager.OrderId)) { - if (order.OrderId != OrderManager.OrderId) throw new OrderException(RobotErrors.Error1001(OrderManager.OrderId, order.OrderId)); - OrderManager.UpdateOrder(order.OrderUpdateId, order.Nodes, order.Edges); + if (ActionManager.HasActionRunning) throw new OrderException(RobotErrors.Error1007()); + if (ErrorManager.HasFatalError) throw new OrderException(RobotErrors.Error1008()); + if (NavigationManager.Driving) throw new OrderException(RobotErrors.Error1009()); } - else if (ActionManager.HasActionRunning) throw new OrderException(RobotErrors.Error1007()); - else if (ErrorManager.HasFatalError) throw new OrderException(RobotErrors.Error1008()); - else if (NavigationManager.Driving) throw new OrderException(RobotErrors.Error1009()); - else OrderManager.StartOrder(order.OrderId, order.Nodes, order.Edges); + else if (order.OrderId != OrderManager.OrderId) throw new OrderException(RobotErrors.Error1001(OrderManager.OrderId, order.OrderId)); + OrderManager.UpdateOrder(order); } - catch (OrderException orEx) + catch (RobotExeption orEx) { if (orEx.Error is not null) { ErrorManager.AddError(orEx.Error, TimeSpan.FromSeconds(10)); - Logger.Warning($"Lỗi khi xử lí Order: {orEx.Error.ErrorDescription}"); + Logger.Warning($"Order mới có lỗi: {orEx.Error.ErrorDescription}"); } + else Logger.Warning($"Order mới có lỗi: {orEx.Message}"); } catch (Exception ex) { @@ -68,21 +69,26 @@ public partial class RobotController(IOrder OrderManager, } } - public void NewInstanceActionUpdated(InstantActionsMsg action) + public void NewInstantActionUpdated(InstantActionsMsg action) { if (NewInstanceMutex.WaitOne(2000)) { try { - + ActionManager.AddInstantAction(action.Actions); } - catch (ActionException acEx) + catch (RobotExeption acEx) { - + if (acEx.Error is not null) + { + ErrorManager.AddError(acEx.Error, TimeSpan.FromSeconds(10)); + Logger.Warning($"InstantAction có lỗi: {acEx.Error.ErrorDescription}"); + } + else Logger.Warning($"InstantAction có lỗi: {acEx.Message}"); } catch (Exception ex) { - + Logger.Warning($"Lỗi khi xử lí InstantAction: {ex.Message}"); } finally { @@ -93,7 +99,11 @@ public partial class RobotController(IOrder OrderManager, public void Pause() { - + if(StateManager.HasState(AutoStateType.Executing.ToString())) + { + if(StateManager.HasState(ExecutingStateType.Moving.ToString())) OrderManager.PauseOrder(); + else if(StateManager.HasState(ExecutingStateType.ACT.ToString())) ActionManager.PauseActions(); + } } public void Resume() diff --git a/RobotApp/Services/Robot/RobotControllerInitialize.cs b/RobotApp/Services/Robot/RobotControllerInitialize.cs index a8a8438..566f478 100644 --- a/RobotApp/Services/Robot/RobotControllerInitialize.cs +++ b/RobotApp/Services/Robot/RobotControllerInitialize.cs @@ -8,6 +8,7 @@ public partial class RobotController private readonly AutoResetEvent StartButtonPressedEvent = new(false); private readonly AutoResetEvent StopButtonPressedEvent = new(false); private readonly AutoResetEvent ResetButtonPressedEvent = new(false); + private async Task InitializationingHandler(CancellationToken cancellationToken) { try @@ -23,9 +24,9 @@ public partial class RobotController if (StateManager.CurrentStateName == Enum.GetName(SystemStateType.Initializing)) { if (PeripheralManager.IsReady && - //PeripheralManager.LiftMotorReady && - //PeripheralManager.LeftMotorReady && - //PeripheralManager.RightMotorReady && + PeripheralManager.LiftMotorReady && + PeripheralManager.LeftMotorReady && + PeripheralManager.RightMotorReady && BatteryManager.IsReady && Localization.IsReady && NavigationManager.IsReady) break; @@ -35,18 +36,22 @@ 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; + ConnectionManager.ActionUpdated += NewInstantActionUpdated; await ConnectionManager.StartConnection(cancellationToken); Logger.Info("Robot đã kết nối tới Fleet Manager."); StateManager.TransitionTo(SystemStateType.Standby); - StartButtonPressedEvent.Reset(); - Logger.Info("Chờ nút Start được nhấn để vào chế độ hoạt động..."); - if (StartButtonPressedEvent.WaitOne()) + if (!RobotConfiguration.IsSimulation) { - StateManager.TransitionTo(RootStateType.Auto); - PeripheralManager.SetSytemState(SystemState.IDLE); + StartButtonPressedEvent.Reset(); + Logger.Info("Chờ nút Start được nhấn để vào chế độ hoạt động..."); + if (StartButtonPressedEvent.WaitOne()) + { + StateManager.TransitionTo(RootStateType.Auto); + PeripheralManager.SetSytemState(SystemState.IDLE); + } } + else StateManager.TransitionTo(RootStateType.Auto); } catch (Exception ex) { @@ -56,6 +61,8 @@ public partial class RobotController private void StopHandler() { + var stopConnection = ConnectionManager.StopConnection(); + stopConnection.Wait(); PeripheralManager.OnPeripheralModeChanged -= SwichModeChanged; PeripheralManager.OnButtonPressed -= OnButtonPressed; PeripheralManager.OnStop -= OnStop; @@ -65,7 +72,7 @@ public partial class RobotController { switch (mode) { - case PeripheralMode.AUTO: + case PeripheralMode.AUTOMATIC: if (StartButtonPressedEvent.WaitOne()) { StateManager.TransitionTo(RootStateType.Auto); diff --git a/RobotApp/Services/Robot/RobotDriver.cs b/RobotApp/Services/Robot/RobotDriver.cs index c950b51..70c7a89 100644 --- a/RobotApp/Services/Robot/RobotDriver.cs +++ b/RobotApp/Services/Robot/RobotDriver.cs @@ -4,11 +4,11 @@ namespace RobotApp.Services.Robot; public class RobotDriver : IDriver { - public bool IsReady => throw new NotImplementedException(); + public bool IsReady { get; private set; } - public double LinearVelocity => throw new NotImplementedException(); + public double LinearVelocity { get; private set; } - public double AngularVelocity => throw new NotImplementedException(); + public double AngularVelocity { get; private set; } public bool ControlVelocity(double left, double right) { diff --git a/RobotApp/Services/Robot/RobotErrors.cs b/RobotApp/Services/Robot/RobotErrors.cs index b6b2189..6ba52f2 100644 --- a/RobotApp/Services/Robot/RobotErrors.cs +++ b/RobotApp/Services/Robot/RobotErrors.cs @@ -5,11 +5,12 @@ namespace RobotApp.Services.Robot; public class RobotErrors : IError { - private readonly List Errors = []; + public Error[] ErrorsState => [.. Errors]; public bool HasFatalError => Errors.Any(e => e.ErrorLevel == ErrorLevel.FATAL.ToString()); - public event Action? OnNewFatalError; + private readonly List Errors = []; + public void AddError(Error error, TimeSpan? clearAfter = null) { if (Errors.Any(e => e.ErrorType == error.ErrorType && e.ErrorHint == error.ErrorHint)) return; diff --git a/RobotApp/Services/Robot/RobotFactsheet.cs b/RobotApp/Services/Robot/RobotFactsheet.cs index 1210f57..e1a43ae 100644 --- a/RobotApp/Services/Robot/RobotFactsheet.cs +++ b/RobotApp/Services/Robot/RobotFactsheet.cs @@ -69,7 +69,7 @@ public class RobotFactsheet(RobotConnection RobotConnection, RobotConfiguration ActionScopes = [ActionScopes.INSTANT.ToString()], ActionParameters = [], ResultDescription = "Robot đã tạm dừng.", - BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString()], + BlockingTypes = [BlockingType.NONE.ToString()], }; public readonly static AgvAction StopPause = new() @@ -79,7 +79,7 @@ public class RobotFactsheet(RobotConnection RobotConnection, RobotConfiguration ActionScopes = [ActionScopes.INSTANT.ToString()], ActionParameters = [], ResultDescription = "Robot đã tiếp tục hoạt động.", - BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString(), BlockingType.HARD.ToString()], + BlockingTypes = [BlockingType.NONE.ToString()], }; public readonly static AgvAction StartCharging = new() diff --git a/RobotApp/Services/Robot/RobotInfomations.cs b/RobotApp/Services/Robot/RobotInfomations.cs index 7a3d478..8edcf6f 100644 --- a/RobotApp/Services/Robot/RobotInfomations.cs +++ b/RobotApp/Services/Robot/RobotInfomations.cs @@ -1,10 +1,13 @@ using RobotApp.Interfaces; +using RobotApp.Services.State; using RobotApp.VDA5050.State; +using RobotApp.VDA5050.Type; namespace RobotApp.Services.Robot; -public class RobotInfomations : IInfomation +public class RobotInfomations() : IInfomation { + public Information[] InformationState => [.. Infors]; private readonly List Infors = []; public void AddInfo(Information infor) { diff --git a/RobotApp/Services/Robot/RobotLoads.cs b/RobotApp/Services/Robot/RobotLoads.cs index 5fe7a8a..84729f7 100644 --- a/RobotApp/Services/Robot/RobotLoads.cs +++ b/RobotApp/Services/Robot/RobotLoads.cs @@ -5,7 +5,7 @@ namespace RobotApp.Services.Robot; public class RobotLoads(IPeripheral PeriperalManager) : ILoad { - public Load? Load => PeriperalManager.HasLoad ? GetLoad() : null; + public Load[] Load => PeriperalManager.HasLoad ? [GetLoad()] : []; private static Load GetLoad() { return new() diff --git a/RobotApp/Services/Robot/RobotLocalization.cs b/RobotApp/Services/Robot/RobotLocalization.cs index 2926cd0..02c139b 100644 --- a/RobotApp/Services/Robot/RobotLocalization.cs +++ b/RobotApp/Services/Robot/RobotLocalization.cs @@ -430,9 +430,15 @@ public class RobotLocalization(RobotConfiguration RobotConfiguration, Simulation XlocData.IsReady = true; } + public double DistanceTo(double x, double y) + { + return MathExtensions.Distance(this.X, this.Y, x, y); + } + public override Task StopAsync(CancellationToken cancellationToken) { ReaderTimer?.Dispose(); + ReaderTimer = null; return base.StopAsync(cancellationToken); } } diff --git a/RobotApp/Services/Robot/RobotNavigation.cs b/RobotApp/Services/Robot/RobotNavigation.cs index 5d3b6c4..adcd22a 100644 --- a/RobotApp/Services/Robot/RobotNavigation.cs +++ b/RobotApp/Services/Robot/RobotNavigation.cs @@ -4,60 +4,65 @@ using RobotApp.VDA5050.Order; namespace RobotApp.Services.Robot; -public class RobotNavigation : INavigation +public class RobotNavigation(RobotConfiguration RobotConfiguration, IServiceProvider ServiceProvider) : INavigation { public bool IsReady => IsSimulation; public bool Driving => IsSimulation ? SimNavigation is not null ? SimNavigation.Driving : false : false; public double VelocityX => IsSimulation ? SimNavigation is not null ? SimNavigation.VelocityX : 0 : 0; public double VelocityY => IsSimulation ? SimNavigation is not null ? SimNavigation.VelocityY : 0 : 0; public double Omega => IsSimulation ? SimNavigation is not null ? SimNavigation.Omega : 0 : 0; - public NavigationState State => IsSimulation ? SimNavigation is not null ? SimNavigation.State : NavigationState.None : NavigationState.None; + public NavigationState State => IsSimulation ? SimNavigation is not null ? SimNavigation.State : NavigationState.Idle : NavigationState.None; - private readonly SimulationNavigation? SimNavigation; - private readonly bool IsSimulation; + private SimulationNavigation? SimNavigation; + private readonly bool IsSimulation = RobotConfiguration.IsSimulation; - public RobotNavigation(RobotConfiguration RobotConfiguration, IServiceProvider ServiceProvider) - { - IsSimulation = RobotConfiguration.IsSimulation; - if (IsSimulation) - { - SimNavigation = SimulationNavigationManager.GetNavigation(RobotConfiguration.SimulationModel.NavigationType, ServiceProvider); - } - } + public event Action? OnNavigationStateChanged; + public event Action? OnNavigationFinished; public void CancelMovement() { - throw new NotImplementedException(); + if (IsSimulation) + { + SimNavigation?.CancelMovement(); + } } public void Move(Node[] nodes, Edge[] edges) { - throw new NotImplementedException(); + if (IsSimulation) + { + SimNavigation = SimulationNavigationManager.GetNavigation(RobotConfiguration.SimulationModel.NavigationType, ServiceProvider); + SimNavigation.OnNavigationFinished += NavigationFinished; + SimNavigation.Move(nodes, edges); + } } public void MoveStraight(double x, double y) { - throw new NotImplementedException(); + if (IsSimulation) + { + SimNavigation = SimulationNavigationManager.GetNavigation(RobotConfiguration.SimulationModel.NavigationType, ServiceProvider); + SimNavigation.MoveStraight(x, y); + } } - public void Paused() + public void Pause() { - throw new NotImplementedException(); + if (IsSimulation) SimNavigation?.Pause(); } public void Resume() { - throw new NotImplementedException(); + if (IsSimulation) SimNavigation?.Resume(); } public void Rotate(double angle) { - throw new NotImplementedException(); - } - - public void UpdateOrder(int lastBaseSequence) - { - throw new NotImplementedException(); + if (IsSimulation) + { + SimNavigation = SimulationNavigationManager.GetNavigation(RobotConfiguration.SimulationModel.NavigationType, ServiceProvider); + SimNavigation.Rotate(angle * 180 / Math.PI); + } } public void RefreshOrder(Node[] nodes, Edge[] edges) @@ -67,11 +72,17 @@ public class RobotNavigation : INavigation public void UpdateOrder(string lastBaseNodeId) { - throw new NotImplementedException(); + if (IsSimulation) SimNavigation?.UpdateOrder(lastBaseNodeId); } public void Refresh() { throw new NotImplementedException(); } + + private void NavigationFinished() + { + OnNavigationFinished?.Invoke(); + if(SimNavigation is not null) SimNavigation.OnNavigationFinished -= NavigationFinished; + } } diff --git a/RobotApp/Services/Robot/RobotOrderController.cs b/RobotApp/Services/Robot/RobotOrderController.cs index 7861839..1783aeb 100644 --- a/RobotApp/Services/Robot/RobotOrderController.cs +++ b/RobotApp/Services/Robot/RobotOrderController.cs @@ -1,193 +1,233 @@ -using RobotApp.Interfaces; +using Microsoft.AspNetCore.Components; +using RobotApp.Common.Shares.Enums; +using RobotApp.Interfaces; using RobotApp.Services.Exceptions; +using RobotApp.Services.State; using RobotApp.VDA5050.Order; using RobotApp.VDA5050.State; using Action = RobotApp.VDA5050.InstantAction.Action; namespace RobotApp.Services.Robot; -public class RobotOrderController(INavigation NavigationManager, IInstantActions ActionManager, IError ErrorManager, Logger Logger) : IOrder +public class RobotOrderController(INavigation NavigationManager, + ILocalization Localization, + IInstantActions ActionManager, + IError ErrorManager, + RobotStateMachine StateManager, + Logger Logger) : IOrder { public string OrderId { get; private set; } = string.Empty; public int OrderUpdateId { get; private set; } - public NodeState[] NodeStates { get; private set; } = []; - public EdgeState[] EdgeStates { get; private set; } = []; - public string LastNodeId { get; private set; } = string.Empty; - public int LastNodeSequenceId { 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 string LastNodeId => LastNode is null ? "" : LastNode.NodeId; + public int LastNodeSequenceId => LastNode is null ? 0 : LastNode.SequenceId; - private readonly Dictionary OrderActions = []; - private readonly Mutex OrderMutex = new(); - - protected const int CycleHandlerMilliseconds = 100; + private const int CycleHandlerMilliseconds = 100; private WatchTimer? OrderTimer; - private int BaseSequenceId = 0; + private readonly Dictionary OrderActions = []; - public void StartOrder(string orderId, Node[] nodes, Edge[] edges) + private Node[] Nodes = []; + private Edge[] Edges = []; + private Node? CurrentBaseNode; + private Node? LastNode; + private readonly Lock LockObject = new(); + private OrderMsg? NewOrder; + + public void UpdateOrder(OrderMsg order) { - if (OrderMutex.WaitOne(2000)) + lock (LockObject) { - try - { - if (!string.IsNullOrEmpty(OrderId) && orderId != OrderId) throw new OrderException(RobotErrors.Error1001(OrderId, orderId)); - if (nodes.Length < 2) throw new OrderException(RobotErrors.Error1002(nodes.Length)); - if (edges.Length < 1) throw new OrderException(RobotErrors.Error1003(edges.Length)); - if (edges.Length != nodes.Length - 1) throw new OrderException(RobotErrors.Error1004(nodes.Length, edges.Length)); - if (NavigationManager.State != NavigationState.Idle) throw new OrderException(RobotErrors.Error1012(NavigationManager.State)); - - for (int i = 0; i < nodes.Length; i++) - { - if (nodes[i].Actions is not null && nodes[i].Actions.Length > 0) - { - foreach (var item in nodes[i].Actions) - { - item.ActionDescription += $"\n NodeId: {nodes[i].NodeId}"; - } - OrderActions.Add(nodes[i].NodeId, nodes[i].Actions); - } - if (i < nodes.Length - 1 && edges[i] is not null && edges[i].Length > 0) - { - foreach (var item in edges[i].Actions) - { - item.ActionDescription += $"\n NodeId: {nodes[i].NodeId}"; - } - OrderActions.TryAdd(nodes[i].NodeId, edges[i].Actions); - } - if (nodes[i].SequenceId != i) throw new OrderException(RobotErrors.Error1010(nodes[i].NodeId, nodes[i].SequenceId, i)); - if (i < nodes.Length - 1 && edges[i].SequenceId != i) throw new OrderException(RobotErrors.Error1011(edges[i].EdgeId, edges[i].SequenceId, i)); - if (nodes[i].Released) BaseSequenceId = nodes[i].SequenceId; - } - - 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 - })]; - - OrderId = orderId; - ActionManager.AddOrderActions([.. OrderActions.Values.SelectMany(a => a)]); - NavigationManager.Move(nodes, edges); - HandleNavigationStart(); - } - catch (OrderException orEx) - { - if (orEx.Error is not null) - { - ErrorManager.AddError(orEx.Error, TimeSpan.FromSeconds(10)); - Logger.Warning($"Lỗi khi khởi tạo Order: {orEx.Error.ErrorDescription}"); - } - } - catch (Exception ex) - { - Logger.Warning($"Lỗi khi khởi tạo Order: {ex.Message}"); - } - finally - { - OrderMutex.ReleaseMutex(); - } + NewOrder = order; } - Logger.Warning($"Lỗi khi khởi tạo Order: có order đang được khởi tạo chưa xong"); - } - - public void UpdateOrder(int orderUpdateId,Node[] nodes, Edge[] edges) - { - if (OrderMutex.WaitOne(2000)) - { - try - { - if (string.IsNullOrEmpty(OrderId)) throw new OrderException(RobotErrors.Error1005()); - if (orderUpdateId > OrderUpdateId) - { - OrderUpdateId = orderUpdateId; - // Check Order Update hợp lệ - // Check Order update Hoziron hay không - } - } - catch (OrderException orEx) - { - if (orEx.Error is not null) - { - ErrorManager.AddError(orEx.Error, TimeSpan.FromSeconds(10)); - Logger.Warning($"Lỗi khi cập nhật Order: {orEx.Error.ErrorDescription}"); - } - } - catch (Exception ex) - { - Logger.Warning($"Lỗi khi cập nhật Order: {ex.Message}"); - } - finally - { - OrderMutex.ReleaseMutex(); - } - } - Logger.Warning($"Lỗi khi cập nhật Order: có order đang được cập nhật chưa xong"); + if (OrderTimer is null) HandleOrderStart(); } public void StopOrder() { - HandleNavigationStop(); NavigationManager.CancelMovement(); - - OrderId = string.Empty; - OrderActions.Clear(); - OrderUpdateId = 0; - BaseSequenceId = 0; - LastNodeSequenceId = 0; - NodeStates = []; - EdgeStates = []; + NavigationFinished(); } public void PauseOrder() { - throw new NotImplementedException(); + NavigationManager.Pause(); } public void ResumeOrder() { - throw new NotImplementedException(); + NavigationManager.Resume(); } - private void HandleNavigationStart() + private void HandleOrderStart() { OrderTimer = new(CycleHandlerMilliseconds, OrderHandler, Logger); OrderTimer.Start(); } - private void HandleNavigationStop() + private void HandleOrderStop() { OrderTimer?.Dispose(); + OrderTimer = null; + } + + private Node? GetCurrentNode() + { + Node? inNode = null; + 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 < minDistance) + { + minDistance = distance; + inNode = node; + } + } + } + return inNode; + } + + private void NavigationFinished() + { + HandleOrderStop(); + OrderId = string.Empty; + OrderUpdateId = 0; + OrderActions.Clear(); + CurrentBaseNode = null; + Nodes = []; + Edges = []; + StateManager.TransitionTo(AutoStateType.Idle); + } + + private void HanleNewOrder(OrderMsg order) + { + if (NavigationManager.State != NavigationState.Idle) throw new OrderException(RobotErrors.Error1012(NavigationManager.State)); + + 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}"; + } + 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) + { + foreach (var item in order.Edges[i].Actions) + { + item.ActionDescription += $"\n NodeId: {order.Nodes[i].NodeId}"; + } + OrderActions.TryAdd(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(); + if (OrderActions.Count > 0) ActionManager.AddOrderActions([.. OrderActions.Values.SelectMany(a => a)]); + NavigationManager.Move(order.Nodes, order.Edges); + NavigationManager.OnNavigationFinished += NavigationFinished; + OrderId = order.OrderId; + OrderUpdateId = order.OrderUpdateId; + 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); + } + + private bool IsNewPath() + { + return true; + } + + private void HandleUpdateOrder(OrderMsg order) + { + if (order.OrderId != OrderId) throw new OrderException(RobotErrors.Error1001(OrderId, order.OrderId)); + if (order.OrderUpdateId <= OrderUpdateId) return; + + var lastBastNode = order.Nodes.Last(n => n.Released); + if (lastBastNode is not null && lastBastNode.NodeId != CurrentBaseNode?.NodeId) + { + CurrentBaseNode = lastBastNode; + NavigationManager.UpdateOrder(CurrentBaseNode.NodeId); + } + Nodes = order.Nodes; + Edges = order.Edges; + OrderUpdateId = order.OrderUpdateId; + } + + private void HandleOrder() + { + var currentNode = GetCurrentNode(); + if (currentNode is not null && currentNode.NodeId != LastNode?.NodeId) + { + LastNode = currentNode; + // xuử lí nếu gặp node mới + } } private void OrderHandler() { - if(!string.IsNullOrEmpty(OrderId) && BaseSequenceId > 0) + try { - if(NavigationManager.State == NavigationState.Initializing) + if (NewOrder is not null) { - // khởi tạo Order - + Console.WriteLine("Has new Order"); + OrderMsg NewOrderHandler; + lock (LockObject) + { + NewOrderHandler = NewOrder; + NewOrder = null; + } + + 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 != NewOrderHandler.Nodes.Length - 1) throw new OrderException(RobotErrors.Error1004(NewOrderHandler.Nodes.Length, NewOrderHandler.Edges.Length)); + + if (string.IsNullOrEmpty(OrderId)) HanleNewOrder(NewOrderHandler); + else HandleUpdateOrder(NewOrderHandler); } - else + HandleOrder(); + } + catch (RobotExeption orEx ) + { + if (orEx.Error is not null) { - // xử lí khi có Order + ErrorManager.AddError(orEx.Error, TimeSpan.FromSeconds(10)); + Logger.Warning($"Lỗi khi xử lí Order: {orEx.Error.ErrorDescription}"); } + else Logger.Warning($"Lỗi khi xử lí Order: {orEx.Message}"); + } + catch (Exception ex) + { + Logger.Warning($"Lỗi khi xử lí Order: {ex.Message}"); } } } diff --git a/RobotApp/Services/Robot/RobotPathPlanner.cs b/RobotApp/Services/Robot/RobotPathPlanner.cs index 0d08ff2..81777fa 100644 --- a/RobotApp/Services/Robot/RobotPathPlanner.cs +++ b/RobotApp/Services/Robot/RobotPathPlanner.cs @@ -4,21 +4,100 @@ using RobotApp.Common.Shares.Model; using RobotApp.Services.Exceptions; using RobotApp.Services.Robot.Simulation.Navigation; using RobotApp.VDA5050.Order; -using RobotApp.VDA5050.State; namespace RobotApp.Services.Robot; public class RobotPathPlanner(IConfiguration Configuration) { private readonly double ResolutionSplit = Configuration.GetValue("PathPlanning:ResolutionSplit", 0.1); + 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) + { + (double futurex, double futurey) = MathExtensions.Curve(0.1, new() + { + X1 = inNode.NodePosition.X, + Y1 = inNode.NodePosition.Y, + X2 = futureNode.NodePosition.X, + Y2 = futureNode.NodePosition.Y, + ControlPoint1X = edge.Trajectory.ControlPoints.Length > 2 ? edge.Trajectory.ControlPoints[1].X : 0, + ControlPoint1Y = edge.Trajectory.ControlPoints.Length > 2 ? edge.Trajectory.ControlPoints[1].Y : 0, + 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) = + ( + inNode.NodePosition.X + Math.Cos(currentTheta * Math.PI / 180), + inNode.NodePosition.Y + Math.Sin(currentTheta * Math.PI / 180) + ); + + var angle = MathExtensions.GetVectorAngle( + inNode.NodePosition.X, + inNode.NodePosition.Y, + robotx, + roboty, + futurex, + futurey); + return angle > ReverseDirectionAngleDegree ? RobotDirection.BACKWARD : RobotDirection.FORWARD; + } public NavigationNode[] GetNavigationNode(double currentTheta, Node[] nodes, Edge[] edges) { if (nodes.Length < 2) throw new PathPlannerException(RobotErrors.Error1002(nodes.Length)); if (edges.Length < 1) throw new PathPlannerException(RobotErrors.Error1003(edges.Length)); if (edges.Length != nodes.Length - 1) throw new PathPlannerException(RobotErrors.Error1004(nodes.Length, edges.Length)); + NavigationNode[] navigationNodes = [.. nodes.Select(n => new NavigationNode() + { + NodeId = n.NodeId, + X = n.NodePosition.X, + Y = n.NodePosition.Y, + Theta = n.NodePosition.Theta, + AllowedDeviationXY = n.NodePosition.AllowedDeviationXY, + AllowedDeviationTheta = n.NodePosition.AllowedDeviationTheta, + Description = n.NodeDescription + })]; + navigationNodes[0].Direction = GetDirectionInNode(currentTheta, nodes[0], nodes[1], edges[0]); + for (int i = 1; i < nodes.Length - 1; i++) + { + (double lastx, double lasty) = MathExtensions.Curve(0.1, new() + { + X1 = nodes[i - 1].NodePosition.X, + Y1 = nodes[i - 1].NodePosition.Y, + X2 = nodes[i].NodePosition.X, + Y2 = nodes[i].NodePosition.Y, + ControlPoint1X = edges[i - 1].Trajectory.ControlPoints.Length > 2 ? edges[i - 1].Trajectory.ControlPoints[1].X : 0, + ControlPoint1Y = edges[i - 1].Trajectory.ControlPoints.Length > 2 ? edges[i - 1].Trajectory.ControlPoints[1].Y : 0, + 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, + Y1 = nodes[i].NodePosition.Y, + X2 = nodes[i + 1].NodePosition.X, + Y2 = nodes[i + 1].NodePosition.Y, + ControlPoint1X = edges[i].Trajectory.ControlPoints.Length > 2 ? edges[i].Trajectory.ControlPoints[1].X : 0, + ControlPoint1Y = edges[i].Trajectory.ControlPoints.Length > 2 ? edges[i].Trajectory.ControlPoints[1].Y : 0, + ControlPoint2X = edges[i].Trajectory.ControlPoints.Length > 3 ? edges[i].Trajectory.ControlPoints[2].X : 0, + ControlPoint2Y = edges[i].Trajectory.ControlPoints.Length > 3 ? edges[i].Trajectory.ControlPoints[2].Y : 0, + TrajectoryDegree = edges[i].Trajectory.Degree == 1 ? TrajectoryDegree.One : edges[i].Trajectory.Degree == 2 ? TrajectoryDegree.Two : TrajectoryDegree.Three, + }); + var angle = MathExtensions.GetVectorAngle( + nodes[i].NodePosition.X, + nodes[i].NodePosition.Y, + lastx, + lasty, + futurex, + futurey); - return []; + if (angle < ReverseDirectionAngleDegree) navigationNodes[i].Direction = navigationNodes[i - 1].Direction == RobotDirection.FORWARD ? RobotDirection.BACKWARD : RobotDirection.FORWARD; + else navigationNodes[i].Direction = navigationNodes[i - 1].Direction; + } + navigationNodes[^1].Direction = navigationNodes[^2].Direction; + return navigationNodes; } public NavigationNode[] PathSplit(NavigationNode[] nodes, Edge[] edges) @@ -27,7 +106,7 @@ public class RobotPathPlanner(IConfiguration Configuration) if (edges.Length < 1) throw new PathPlannerException(RobotErrors.Error1003(edges.Length)); if (edges.Length != nodes.Length - 1) throw new PathPlannerException(RobotErrors.Error1004(nodes.Length, edges.Length)); - List navigationNode = [new() + List navigationNode = [new() { NodeId = nodes[0].NodeId, X = nodes[0].X, @@ -49,11 +128,11 @@ public class RobotPathPlanner(IConfiguration Configuration) Y1 = startNode.Y, X2 = endNode.X, Y2 = endNode.Y, - ControlPoint1X = edge.Trajectory.ControlPoints.Length > 0 ? edge.Trajectory.ControlPoints[0].X : 0, - ControlPoint1Y = edge.Trajectory.ControlPoints.Length > 0 ? edge.Trajectory.ControlPoints[0].Y : 0, - ControlPoint2X = edge.Trajectory.ControlPoints.Length > 1 ? edge.Trajectory.ControlPoints[1].X : 0, - ControlPoint2Y = edge.Trajectory.ControlPoints.Length > 1 ? edge.Trajectory.ControlPoints[1].Y : 0, - TrajectoryDegree = edge.Trajectory.Degree == 1 ? TrajectoryDegree.One : edge.Trajectory.Degree == 2 ? TrajectoryDegree.Two :TrajectoryDegree.Three + ControlPoint1X = edge.Trajectory.ControlPoints.Length > 2 ? edge.Trajectory.ControlPoints[1].X : 0, + ControlPoint1Y = edge.Trajectory.ControlPoints.Length > 2 ? edge.Trajectory.ControlPoints[1].Y : 0, + 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 length = EdgeCalculatorModel.GetEdgeLength(); @@ -83,6 +162,6 @@ public class RobotPathPlanner(IConfiguration Configuration) Description = nodes[0].Description }); } - return [..navigationNode]; + return [.. navigationNode]; } } diff --git a/RobotApp/Services/Robot/RobotPeripheral.cs b/RobotApp/Services/Robot/RobotPeripheral.cs index 7d63cd8..9f89a48 100644 --- a/RobotApp/Services/Robot/RobotPeripheral.cs +++ b/RobotApp/Services/Robot/RobotPeripheral.cs @@ -5,7 +5,7 @@ namespace RobotApp.Services.Robot; public partial class RobotPeripheral(RobotConfiguration RobotConfiguration, IError ErrorManager, Logger Logger) : BackgroundService, IPeripheral, ISafety { - public bool IsReady { get; private set; } = false; + public bool IsReady { get; private set; } = RobotConfiguration.IsSimulation; public event Action? OnSafetySpeedChanged; public event Action? OnPeripheralModeChanged; @@ -17,26 +17,31 @@ public partial class RobotPeripheral(RobotConfiguration RobotConfiguration, IErr public void SetHorizontalLoad(bool value) { + if(RobotConfiguration.IsSimulation) return; WritePLC(client => client.WriteSingleCoil(SetHorizontalLoadAddress, value)); } public void SetMutedBase(bool muted) { + if (RobotConfiguration.IsSimulation) return; WritePLC(client => client.WriteSingleCoil(SetMutedBaseAddress, muted)); } public void SetMutedLoad(bool muted) { + if (RobotConfiguration.IsSimulation) return; WritePLC(client => client.WriteSingleCoil(SetMutedLoadAddress, muted)); } public void SetOnCharging(bool value) { + if (RobotConfiguration.IsSimulation) return; WritePLC(client => client.WriteSingleCoil(EnableChargingAddress, value)); } public void SetProccessState(ProccessingState state) { + if (RobotConfiguration.IsSimulation) return; WritePLC(client => { switch (state) @@ -61,6 +66,7 @@ public partial class RobotPeripheral(RobotConfiguration RobotConfiguration, IErr public void SetSytemState(SystemState state) { + if (RobotConfiguration.IsSimulation) return; WritePLC(client => { if (state != SystemState.ERROR) client.WriteSingleCoil(RobotStateErrorAddress, false); @@ -139,9 +145,9 @@ public partial class RobotPeripheral(RobotConfiguration RobotConfiguration, IErr { PeripheralMode = PeripheralMode.SERVICE; } - else if (switchs[1] && PeripheralMode != PeripheralMode.AUTO) + else if (switchs[1] && PeripheralMode != PeripheralMode.AUTOMATIC) { - PeripheralMode = PeripheralMode.AUTO; + PeripheralMode = PeripheralMode.AUTOMATIC; } else if (switchs[2] && PeripheralMode != PeripheralMode.MANUAL) { @@ -196,6 +202,8 @@ public partial class RobotPeripheral(RobotConfiguration RobotConfiguration, IErr LiftedUp = sensors[2]; LiftedDown = sensors[3]; LiftHome = sensors[4]; + if(Emergency) OnStop?.Invoke(StopStateType.EMC); + else if (Bumper) OnStop?.Invoke(StopStateType.Bumber); } } @@ -262,6 +270,7 @@ public partial class RobotPeripheral(RobotConfiguration RobotConfiguration, IErr { await Task.Yield(); PeripheralReaderTimer?.Dispose(); + PeripheralReaderTimer = null; IsReady = false; _ = base.StopAsync(cancellationToken); } diff --git a/RobotApp/Services/Robot/RobotPeripheralData.cs b/RobotApp/Services/Robot/RobotPeripheralData.cs index f84eb4c..1ded367 100644 --- a/RobotApp/Services/Robot/RobotPeripheralData.cs +++ b/RobotApp/Services/Robot/RobotPeripheralData.cs @@ -4,7 +4,7 @@ namespace RobotApp.Services.Robot; public partial class RobotPeripheral { - public PeripheralMode PeripheralMode { get; private set; } = PeripheralMode.SERVICE; + public PeripheralMode PeripheralMode { get; private set; } = RobotConfiguration.IsSimulation ? PeripheralMode.AUTOMATIC : PeripheralMode.SERVICE; public bool Emergency { get; private set; } public bool Bumper { get; private set; } public bool LiftedUp { get; private set; } @@ -15,11 +15,11 @@ public partial class RobotPeripheral public bool LidarBackProtectField { get; private set; } public bool LidarFrontTimProtectField { get; private set; } - public bool LeftMotorReady { get; private set; } - public bool RightMotorReady { get; private set; } - public bool LiftMotorReady { get; private set; } + public bool LeftMotorReady { get; private set; } = RobotConfiguration.IsSimulation; + public bool RightMotorReady { get; private set; } = RobotConfiguration.IsSimulation; + public bool LiftMotorReady { get; private set; } = RobotConfiguration.IsSimulation; - public bool ButtonStart { get; private set; } + public bool ButtonStart { get; private set; } = RobotConfiguration.IsSimulation; public bool ButtonStop { get; private set; } public bool ButtonReset { get; private set; } diff --git a/RobotApp/Services/Robot/RobotStates.cs b/RobotApp/Services/Robot/RobotStates.cs index b23014c..91a7513 100644 --- a/RobotApp/Services/Robot/RobotStates.cs +++ b/RobotApp/Services/Robot/RobotStates.cs @@ -1,15 +1,26 @@ using RobotApp.Common.Shares; using RobotApp.Interfaces; +using RobotApp.Services.State; using RobotApp.VDA5050; using RobotApp.VDA5050.State; +using RobotApp.VDA5050.Type; using System.Text.Json; namespace RobotApp.Services.Robot; -public class RobotStates(RobotConfiguration RobotConfiguration, RobotConnection ConnectionManager, +public class RobotStates(RobotConfiguration RobotConfiguration, + RobotConnection ConnectionManager, + RobotStateMachine StateManager, Logger Logger, IOrder OrderManager, - IPeripheral PeripheralManager) : BackgroundService + IInstantActions ActionManager, + IPeripheral PeripheralManager, + IInfomation InfoManager, + IError ErrorManager, + ILocalization LocalizationManager, + IBattery BatteryManager, + ILoad LoadManager, + IDriver DriverManager) : BackgroundService { private uint HeaderId = 0; private readonly string SerialNumber = RobotConfiguration.SerialNumber; @@ -43,9 +54,59 @@ public class RobotStates(RobotConfiguration RobotConfiguration, RobotConnection NewBaseRequest = false, DistanceSinceLastNode = 0, OperatingMode = PeripheralManager.PeripheralMode.ToString(), + NodeStates = OrderManager.NodeStates, + EdgeStates = OrderManager.EdgeStates, + ActionStates = ActionManager.ActionStates, + Information = [General, ..InfoManager.InformationState], + Errors = ErrorManager.ErrorsState, + AgvPosition = new() + { + X = LocalizationManager.X, + Y = LocalizationManager.Y, + Theta = LocalizationManager.Theta, + LocalizationScore = LocalizationManager.MatchingScore, + MapId = LocalizationManager.CurrentActiveMap, + DeviationRange = LocalizationManager.Reliability, + PositionInitialized = LocalizationManager.IsReady, + }, + BatteryState = new() + { + Charging = BatteryManager.IsCharging, + BatteryHealth = BatteryManager.SOH, + Reach = BatteryManager.RemainingCapacity, + BatteryVoltage = BatteryManager.Voltage, + BatteryCharge = BatteryManager.SOC, + }, + Loads = LoadManager.Load, + Velocity = new() + { + Vx = DriverManager.LinearVelocity, + Vy = 0, + Omega = DriverManager.AngularVelocity, + }, + SafetyState = new() + { + FieldViolation = PeripheralManager.LidarBackProtectField || PeripheralManager.LidarFrontProtectField || PeripheralManager.LidarFrontTimProtectField, + EStop = PeripheralManager.Emergency || PeripheralManager.Bumper ? EStop.AUTOACK.ToString() : EStop.NONE.ToString(), + } }; } + private Information General => new() + { + InfoType = InformationType.robot_general.ToString(), + InfoDescription = "Thông tin chung của robot", + InfoLevel = InfoLevel.INFO.ToString(), + InfoReferences = + [ + new InfomationReferences + { + ReferenceKey = InformationReferencesKey.robot_state.ToString(), + ReferenceValue = StateManager.CurrentStateName, + }, + ], + }; + private async Task UpdateStateHandler() { await PubState(); @@ -69,6 +130,7 @@ public class RobotStates(RobotConfiguration RobotConfiguration, RobotConnection public override Task StopAsync(CancellationToken cancellationToken) { UpdateStateTimer?.Dispose(); + UpdateStateTimer = null; return base.StopAsync(cancellationToken); } } diff --git a/RobotApp/Services/Robot/RobotVisualization.cs b/RobotApp/Services/Robot/RobotVisualization.cs index bddbc6f..0fd388b 100644 --- a/RobotApp/Services/Robot/RobotVisualization.cs +++ b/RobotApp/Services/Robot/RobotVisualization.cs @@ -62,6 +62,7 @@ public class RobotVisualization(ILocalization Localization, INavigation Navigati public override Task StopAsync(CancellationToken cancellationToken) { UpdateTimer?.Dispose(); + UpdateTimer = null; return base.StopAsync(cancellationToken); } } diff --git a/RobotApp/Services/Robot/Simulation/Navigation/Algorithm/PID.cs b/RobotApp/Services/Robot/Simulation/Navigation/Algorithm/PID.cs index 52f7ee2..2f063dd 100644 --- a/RobotApp/Services/Robot/Simulation/Navigation/Algorithm/PID.cs +++ b/RobotApp/Services/Robot/Simulation/Navigation/Algorithm/PID.cs @@ -29,7 +29,7 @@ public class PID return this; } - public double PID_step(double error, double min, double max, double timeSample) + public double PID_step(double error, double max, double min, double timeSample) { DateTime CurrentTime = DateTime.Now; double P_part = Kp * (error - Pre_Error); diff --git a/RobotApp/Services/Robot/Simulation/Navigation/Algorithm/PurePursuit.cs b/RobotApp/Services/Robot/Simulation/Navigation/Algorithm/PurePursuit.cs index ee69fbe..5e91538 100644 --- a/RobotApp/Services/Robot/Simulation/Navigation/Algorithm/PurePursuit.cs +++ b/RobotApp/Services/Robot/Simulation/Navigation/Algorithm/PurePursuit.cs @@ -7,7 +7,6 @@ public class PurePursuit private double LookaheadDistance = 0.5; private List Waypoints_Value = []; public int OnNodeIndex = 0; - public int GoalIndex = 0; public NavigationNode? Goal; public PurePursuit WithLookheadDistance(double distance) @@ -33,10 +32,10 @@ public class PurePursuit Waypoints_Value = [.. path]; } - public void UpdateGoal(NavigationNode goal) + public void UpdateGoal(string goalId) { - Goal = goal; - GoalIndex = Waypoints_Value.IndexOf(Goal); + var goal = Waypoints_Value.FirstOrDefault(n => n.NodeId == goalId); + if (goal is not null) Goal = goal; } public (NavigationNode node, int index) GetOnNode(double x, double y) diff --git a/RobotApp/Services/Robot/Simulation/Navigation/DifferentialNavigation.cs b/RobotApp/Services/Robot/Simulation/Navigation/DifferentialNavigation.cs index da3a4d4..30de325 100644 --- a/RobotApp/Services/Robot/Simulation/Navigation/DifferentialNavigation.cs +++ b/RobotApp/Services/Robot/Simulation/Navigation/DifferentialNavigation.cs @@ -65,14 +65,13 @@ public class DifferentialNavigation : SimulationNavigation VelocityController.SetSpeed(AngularVelocityLeft, AngularVelocityRight, CycleHandlerMilliseconds / 1000.0); } else if (DistanceToGoal < 0.02) Dispose(); - else NavState = NavigationState.Waiting; } } } } catch (Exception ex) { - Logger.Write($"Error in DifferentialNavigation: {ex.Message}", LogLevel.Error); + Logger.Error($"Error in DifferentialNavigation: {ex.Message}"); } } } diff --git a/RobotApp/Services/Robot/Simulation/Navigation/SimulationNavigation.cs b/RobotApp/Services/Robot/Simulation/Navigation/SimulationNavigation.cs index cf5f116..a517259 100644 --- a/RobotApp/Services/Robot/Simulation/Navigation/SimulationNavigation.cs +++ b/RobotApp/Services/Robot/Simulation/Navigation/SimulationNavigation.cs @@ -16,7 +16,10 @@ public class SimulationNavigation : INavigation, IDisposable public double VelocityY => Visualization.Vy; public double Omega => Visualization.Omega; - protected NavigationState NavState = NavigationState.None; + public event Action? OnNavigationStateChanged; + public event Action? OnNavigationFinished; + + protected NavigationState NavState = NavigationState.Idle; protected bool NavDriving = false; protected const int CycleHandlerMilliseconds = 50; @@ -38,11 +41,13 @@ public class SimulationNavigation : INavigation, IDisposable protected readonly SimulationVelocity VelocityController; protected readonly RobotConfiguration RobotConfiguration; protected readonly RobotPathPlanner PathPlanner; + protected NavigationNode[] NavigationPath = []; protected NavigationNode? CurrentBaseNode; protected NavigationState ResumeState = NavigationState.None; private readonly Logger Logger; + private bool IsIdle => NavState == NavigationState.Idle || NavState == NavigationState.Canceled; public SimulationNavigation(IServiceProvider ServiceProvider) @@ -53,6 +58,7 @@ public class SimulationNavigation : INavigation, IDisposable RobotConfiguration = scope.ServiceProvider.GetRequiredService(); PathPlanner = scope.ServiceProvider.GetRequiredService(); VelocityController = new(Visualization, RobotConfiguration.SimulationModel); + AngularVelocity = RobotConfiguration.SimulationModel.MaxAngularVelocity * RobotConfiguration.SimulationModel.Width / 2 / 2 / RobotConfiguration.SimulationModel.RadiusWheel; } protected void HandleNavigationStart() @@ -64,6 +70,7 @@ public class SimulationNavigation : INavigation, IDisposable protected void HandleNavigationStop() { NavigationTimer?.Dispose(); + NavigationTimer = null; } protected virtual void NavigationHandler() { } @@ -71,7 +78,6 @@ public class SimulationNavigation : INavigation, IDisposable public void CancelMovement() { Dispose(); - NavState = NavigationState.Canceled; } public void Move(Node[] nodes, Edge[] edges) @@ -140,7 +146,7 @@ public class SimulationNavigation : INavigation, IDisposable UpdateOrder(goalNode.NodeId); } - public void Paused() + public void Pause() { ResumeState = State; NavState = NavigationState.Paused; @@ -154,7 +160,7 @@ public class SimulationNavigation : INavigation, IDisposable public void Rotate(double angle) { - if (!IsIdle) throw new SimulationException(RobotErrors.Error1012(NavState)); + if (!IsIdle && NavState!= NavigationState.Initializing) throw new SimulationException(RobotErrors.Error1012(NavState)); RotatePID = new PID().WithKp(10).WithKi(0.01).WithKd(0.1); TargetAngle = MathExtensions.NormalizeAngle(angle); NavState = NavigationState.Rotating; @@ -163,6 +169,12 @@ public class SimulationNavigation : INavigation, IDisposable public void UpdateOrder(string lastBaseNodeId) { + var lastBaseNode = SimulationOrderNodes.FirstOrDefault(n => n.NodeId == lastBaseNodeId); + if (lastBaseNode is not null && lastBaseNode.NodeId != CurrentBaseNode?.NodeId) + { + CurrentBaseNode = lastBaseNode; + MovePurePursuit?.UpdateGoal(lastBaseNode.NodeId); + } } public void RefreshOrder(Node[] nodes, Edge[] edges) @@ -176,9 +188,12 @@ public class SimulationNavigation : INavigation, IDisposable VelocityController.Stop(); OrderNodes = []; OrderEdges = []; + CurrentBaseNode = null; + NavigationPath = []; SimulationOrderNodes = []; NavDriving = false; - NavState = NavigationState.None; + NavState = NavigationState.Idle; + OnNavigationFinished?.Invoke(); GC.SuppressFinalize(this); } diff --git a/RobotApp/Services/Robot/Simulation/SimulationModel.cs b/RobotApp/Services/Robot/Simulation/SimulationModel.cs index e26f400..9421c9c 100644 --- a/RobotApp/Services/Robot/Simulation/SimulationModel.cs +++ b/RobotApp/Services/Robot/Simulation/SimulationModel.cs @@ -8,7 +8,7 @@ public class SimulationModel public readonly double Width = 0.606; public readonly double Length = 1.106; public readonly double MaxVelocity = 1.5; - public readonly double MaxAngularVelocity = 0.3; + public readonly double MaxAngularVelocity = 0.5; public readonly double Acceleration = 2; public readonly double Deceleration = 10; public readonly NavigationType NavigationType = NavigationType.Differential; diff --git a/RobotApp/Services/RobotExtensions.cs b/RobotApp/Services/RobotExtensions.cs index 2203a8a..efa448a 100644 --- a/RobotApp/Services/RobotExtensions.cs +++ b/RobotApp/Services/RobotExtensions.cs @@ -13,20 +13,23 @@ public static class RobotExtensions services.AddSingleton(); services.AddSingleton(); services.AddSingleton(); - + services.AddSingleton(); services.AddInterfaceServiceSingleton(); services.AddInterfaceServiceSingleton(); services.AddInterfaceServiceSingleton(); services.AddInterfaceServiceSingleton(); - services.AddInterfaceServiceSingleton(); services.AddInterfaceServiceSingleton(); services.AddInterfaceServiceSingleton(); + services.AddInterfaceServiceSingleton(); services.AddHostedInterfaceServiceSingleton(); services.AddHostedInterfaceServiceSingleton(); + services.AddHostedInterfaceServiceSingleton(); services.AddHostedServiceSingleton(); services.AddHostedServiceSingleton(); + services.AddHostedServiceSingleton(); + services.AddHostedServiceSingleton(); return services; } diff --git a/RobotApp/Services/State/IRobotState.cs b/RobotApp/Services/State/IRobotState.cs index 8b401a6..6a67e40 100644 --- a/RobotApp/Services/State/IRobotState.cs +++ b/RobotApp/Services/State/IRobotState.cs @@ -13,7 +13,6 @@ public interface IRobotState event Action? OnEnter; event Action? OnExit; - event Action? OnTransition; void Enter(); void Exit(); bool CanTransitionTo(IRobotState targetState); diff --git a/RobotApp/Services/State/RobotStateMachine.cs b/RobotApp/Services/State/RobotStateMachine.cs index fd698b6..6e3754a 100644 --- a/RobotApp/Services/State/RobotStateMachine.cs +++ b/RobotApp/Services/State/RobotStateMachine.cs @@ -49,7 +49,7 @@ public record RobotStateMachine(Logger Logger) : IDisposable { lock (StateLock) { - + } } @@ -302,7 +302,7 @@ public record RobotStateMachine(Logger Logger) : IDisposable { var stopState = GetState(RootStateType.Stop); var subStopState = stopState.SubStates.FirstOrDefault(s => s.Name.Equals(stopType)); - if(subStopState is not null) + if (subStopState is not null) { var transitionToStop = TransitionTo(RootStateType.Stop); if (transitionToStop) @@ -343,6 +343,19 @@ public record RobotStateMachine(Logger Logger) : IDisposable return current.Name.ToString(); } + public bool HasState(string stateName) + { + if (CurrentState == null || string.IsNullOrEmpty(stateName)) return false; + + var current = CurrentState; + while (current != null) + { + if (current.Name.ToString() == stateName) return true; + current = current.SuperState; + } + return false; + } + public string GetCurrentStatePath() { if (CurrentState == null) return "Unknown"; diff --git a/RobotApp/appsettings.json b/RobotApp/appsettings.json index ce4af3a..14fcc85 100644 --- a/RobotApp/appsettings.json +++ b/RobotApp/appsettings.json @@ -10,15 +10,9 @@ } }, "AllowedHosts": "*", - "Simulation": { - "Enable": true, - "RadiusWheel": 0.1, - "Width": 0.606, - "Length": 1.106, - "MaxVelocity": 1.5, - "MaxAngularVelocity": 0.3, - "Acceleration": 2, - "Deceleration": 10, - "NavigationType": "Differential" + "PathPlanning": { + "ResolutionSplit": 0.1, + "ReverseDirectionAngleDegree": 89, + "Ratio": 0.1 } }