diff --git a/RobotApp.VDA5050/Factsheet/AgvActions.cs b/RobotApp.VDA5050/Factsheet/AgvActions.cs index 22b520a..ee08f61 100644 --- a/RobotApp.VDA5050/Factsheet/AgvActions.cs +++ b/RobotApp.VDA5050/Factsheet/AgvActions.cs @@ -9,7 +9,8 @@ public enum ActionScopes NODE, EDGE, } -public class AgvActions + +public class AgvAction { [Required] public string ActionType { get; set; } = string.Empty; diff --git a/RobotApp.VDA5050/Factsheet/ProtocolFeatures.cs b/RobotApp.VDA5050/Factsheet/ProtocolFeatures.cs index ac67457..7b9f744 100644 --- a/RobotApp.VDA5050/Factsheet/ProtocolFeatures.cs +++ b/RobotApp.VDA5050/Factsheet/ProtocolFeatures.cs @@ -8,5 +8,5 @@ public class ProtocolFeatures [Required] public OptionalParameters[] OptionalParameters { get; set; } = []; [Required] - public AgvActions[] AgvActions { get; set; } = []; + public AgvAction[] AgvActions { get; set; } = []; } diff --git a/RobotApp.VDA5050/Type/ActionType.cs b/RobotApp.VDA5050/Type/ActionType.cs index 4b56503..7b58bd5 100644 --- a/RobotApp.VDA5050/Type/ActionType.cs +++ b/RobotApp.VDA5050/Type/ActionType.cs @@ -2,16 +2,32 @@ public enum ActionType { - START_PAUSE, - STOP_PAUSE, - START_CHARGING, - STOP_CHARGING, - INITIALIZATION_POSITION, - PICK, - DROP, - CANCEL_ORDER, - ROTATE, - REQUEST_FACTSHEET, - REQUEST_VISUALIZATION, - REQUEST_STATE, -} + startPause, + stopPause, + startCharging, + stopCharging, + initPosition, + stateRequest, + factsheetRequest, + + logReport, + pick, + drop, + detectObject, + finePositioning, + waitForTrigger, + + cancelOrder, + liftUp, + liftDown, + liftRotate, + rotate, + rotateKeepLift, + mutedBaseOn, + mutedBaseOff, + mutedLoadOn, + mutedLoadOff, + dockTo, + moveStraightToCoor, + moveStraightWithDistance, +} \ No newline at end of file diff --git a/RobotApp/Interfaces/IInstantActions.cs b/RobotApp/Interfaces/IInstantActions.cs index 6e6ab98..d5ea547 100644 --- a/RobotApp/Interfaces/IInstantActions.cs +++ b/RobotApp/Interfaces/IInstantActions.cs @@ -7,8 +7,10 @@ public interface IInstantActions { ActionState[] ActionStates { get; } bool HasActionRunning { get; } - bool AddOrderActions(Action[] actions); - bool StartAction(string actionId); - bool AddInstanceAction(Action action); - bool StopAction(); + void AddOrderActions(Action[] actions); + void AddInstantAction(Action action); + void StartOrderAction(string actionId, bool wait); + void ClearInstantActions(); + void PauseActions(); + void ResumeActions(); } diff --git a/RobotApp/Interfaces/ILocalization.cs b/RobotApp/Interfaces/ILocalization.cs index 9a22d40..dc24872 100644 --- a/RobotApp/Interfaces/ILocalization.cs +++ b/RobotApp/Interfaces/ILocalization.cs @@ -60,11 +60,11 @@ public interface ILocalization MessageResult SetInitializePosition(double x, double y, double theta); /// - /// Bắt đầu quá trình lập bản đồ với độ phân giải chỉ định. + /// Bắt đầu quá trình lập bản đồ. /// /// đơn vị mét/px /// - MessageResult StartMapping(double resolution); + MessageResult StartMapping(); /// /// Dừng quá trình lập bản đồ hiện tại và lưu bản đồ với tên chỉ định. @@ -100,7 +100,7 @@ public interface ILocalization /// /// /// - MessageResult SwitchMap(string mapName, double x, double y, double theta); + MessageResult SwitchMap(string mapName, bool useInitialPose, double x, double y, double theta); /// /// Thay đổi gốc tọa độ của bản đồ hiện tại. @@ -110,4 +110,22 @@ public interface ILocalization /// /// MessageResult ChangeMapOrigin(double x, double y, double theta); + + /// + /// Bắt đầu quá trình cập nhật bản đồ. + /// + /// + MessageResult StartUpdateMap(); + + /// + /// Kết thúc quá trình cập nhật bản đồ. + /// + /// + MessageResult StopUpdateMap(bool save); + + /// + /// Xóa bỏ các lỗi Slam + /// + /// + MessageResult ResetSlamError(); } diff --git a/RobotApp/Interfaces/IOrder.cs b/RobotApp/Interfaces/IOrder.cs index fc6c16e..9e88935 100644 --- a/RobotApp/Interfaces/IOrder.cs +++ b/RobotApp/Interfaces/IOrder.cs @@ -16,4 +16,6 @@ public interface IOrder void StartOrder(string orderId, Node[] nodes, Edge[] edges); void UpdateOrder(int orderUpdateId, Node[] nodes, Edge[] edges); void StopOrder(); + void PauseOrder(); + void ResumeOrder(); } diff --git a/RobotApp/Protos/xloc.proto b/RobotApp/Protos/xloc.proto new file mode 100644 index 0000000..9a9292b --- /dev/null +++ b/RobotApp/Protos/xloc.proto @@ -0,0 +1,285 @@ +syntax = "proto3"; + +package xloc; + +import "google/protobuf/timestamp.proto"; +import "google/protobuf/duration.proto"; +import "google/protobuf/empty.proto"; + +// Std Messages +message Header +{ + uint32 seq = 1; + google.protobuf.Timestamp stamp = 2; + string frame_id = 3; +} +message ColorRGBA +{ + float r = 1; + float g = 2; + float b = 3; + float a = 4; +} + +// Geometry Messages +message Point{ + double x = 1; + double y = 2; + double z = 3; +} +message Point32 +{ + float x = 1; + float y = 2; + float z = 3; +} +message PointStamped +{ + Header header = 1; + Point point = 2; +} +message Quaternion +{ + double x = 1; + double y = 2; + double z = 3; + double w = 4; +} +message Pose +{ + Point position = 1; + Quaternion orientation = 2; +} +message Pose2D +{ + double x = 1; + double y = 2; + double theta = 3; +} +message PoseArray +{ + Header header = 1; + repeated Pose poses = 2; +} +message PoseStamped +{ + Header header = 1; + Pose pose = 2; +} +message Vector3 +{ + double x = 1; + double y = 2; + double z = 3; +} +message Transform +{ + Vector3 translation = 1; + Quaternion rotation = 2; +} +message TransformStamped +{ + Header header = 1; + string child_frame_id = 2; + Transform transform = 3; +} + +// Nav Messages +message MapMetaData +{ + google.protobuf.Timestamp map_load_time = 1; + float resolution = 2; + uint32 width = 3; + uint32 height = 4; + Point origin = 5; +} +message OccupancyGrid +{ + Header header = 1; + MapMetaData info = 2; + repeated int32 data = 3; +} + +// Visualization Messages +message Marker +{ + enum MarkerType + { + ARROW = 0; + CUBE = 1; + SPHERE = 2; + CYLINDER = 3; + LINE_STRIP = 4; + LINE_LIST = 5; + CUBE_LIST = 6; + SPHERE_LIST = 7; + POINTS = 8; + TEXT_VIEW_FACING = 9; + MESH_RESOURCE = 10; + TRIANGLE_LIST = 11; + } + enum Action + { + ADD = 0; + MODIFY = 1; + DELETE = 2; + DELETEALL = 3; + } + Header header = 1; + string ns = 2; + int32 id = 3; + MarkerType type = 4; + Action action = 5; + Pose pose = 6; + Vector3 scale = 7; + ColorRGBA color = 8; + google.protobuf.Duration lifetime = 9; + bool frame_locked = 10; + repeated Point points = 11; + repeated ColorRGBA colors = 12; + string text = 13; + string mesh_resource = 14; + bool mesh_use_embedded_materials = 15; +} +message MarkerArray +{ + repeated Marker markers = 1; +} + +// XLOC Custom Messages +message StatusResponse +{ + enum StatusCode + { + OK=0; + CANCELLED=1; + UNKNOWN=2; + INVALID_ARGUMENT=3; + DEADLINE_EXCEEDED=4; + NOT_FOUND=5; + ALREADY_EXISTS=6; + PERMISSION_DENIED=7; + RESOURCE_EXHAUSTED=8; + FAILED_PRECONDITION=9; + ABORTED=10; + OUT_OF_RANGE=11; + UNIMPLEMENTED=12; + INTERNAL=13; + UNAVAILABLE=14; + DATA_LOSS=15; + } + StatusCode code = 1; + string message = 2; +} + +message XlocDiagnostics +{ + enum SlamState + { + MAPPING = 0; + LOCALIZATION = 1; + PROCESSING = 2; + READY = 3; + ERROR = 4; + } + Header header = 1; + SlamState slam_state = 2; + string slam_state_detail = 3; + string current_active_map = 4; + float reliability = 5; + float matching_score = 6; +} + +// Service definitions + +message ActivateMapRequest +{ + string map_file_name = 1; +} +message ActivateMapResponse +{ + StatusResponse status = 1; +} +message SwitchMapRequest +{ + string map_file_name = 1; + bool use_initial_pose = 2; + Pose initial_pose = 3; +} +message SwitchMapResponse +{ + StatusResponse status = 1; +} +message StartMappingResponse +{ + StatusResponse status = 1; +} +message StopMappingRequest +{ + string save_map_filename = 1; +} +message StopMappingResponse +{ + StatusResponse status = 1; +} +message StartLocalizationResponse +{ + StatusResponse status = 1; + int32 trajectory_id = 2; +} +message StopLocalizationResponse +{ + StatusResponse status = 1; +} +message ChangeMapOriginRequest +{ + Pose new_map_origin = 1; +} +message ChangeMapOriginResponse +{ + StatusResponse status = 1; +} +message StartUpdateMapResponse +{ + StatusResponse status = 1; +} +message StopUpdateMapRequest +{ + bool save_updated_map = 1; +} +message StopUpdateMapResponse +{ + StatusResponse status = 1; +} +message SetInitialPoseRequest +{ + Pose initial_pose = 1; +} +message SetInitialPoseResponse +{ + StatusResponse status = 1; +} +message ResetSlamErrorResponse +{ + StatusResponse status = 1; +} + +service XlocServices { + rpc ActivateMap (ActivateMapRequest) returns (ActivateMapResponse); + rpc SwitchMap (SwitchMapRequest) returns (SwitchMapResponse); + rpc StartMapping (google.protobuf.Empty) returns (StartMappingResponse); + rpc StopMapping (StopMappingRequest) returns (StopMappingResponse); + rpc StartLocalization (google.protobuf.Empty) returns (StartLocalizationResponse); + rpc StopLocalization (google.protobuf.Empty) returns (StopLocalizationResponse); + rpc ChangeMapOrigin (ChangeMapOriginRequest) returns (ChangeMapOriginResponse); + rpc StartUpdateMap (google.protobuf.Empty) returns (StartUpdateMapResponse); + rpc StopUpdateMap (StopUpdateMapRequest) returns (StopUpdateMapResponse); + rpc SetInitialPose (SetInitialPoseRequest) returns (SetInitialPoseResponse); + rpc ResetSlamError (google.protobuf.Empty) returns (ResetSlamErrorResponse); + rpc GetPose (google.protobuf.Empty) returns (stream PoseStamped); + rpc GetStaticMap (google.protobuf.Empty) returns (OccupancyGrid); + rpc GetStreamMap (google.protobuf.Empty) returns (stream OccupancyGrid); + rpc GetTrajectoryNodeListMarker (google.protobuf.Empty) returns (stream MarkerArray); + rpc GetConstraintListMarker (google.protobuf.Empty) returns (stream MarkerArray); + rpc GetDiagnostics (google.protobuf.Empty) returns (stream XlocDiagnostics); +} \ No newline at end of file diff --git a/RobotApp/RobotApp.csproj b/RobotApp/RobotApp.csproj index b89db01..e73f004 100644 --- a/RobotApp/RobotApp.csproj +++ b/RobotApp/RobotApp.csproj @@ -1,36 +1,45 @@ - + - - net9.0 - enable - enable - aspnet-RobotApp-1f61caa2-bbbb-40cd-88b6-409b408a84ea - + + net9.0 + enable + enable + aspnet-RobotApp-1f61caa2-bbbb-40cd-88b6-409b408a84ea + - - - - - - + + + + + + - - - - - - - - - - all - runtime; build; native; contentfiles; analyzers; buildtransitive - - - - - - - + + + + + + + all + runtime; build; native; contentfiles; analyzers; buildtransitive + + + + + + + + all + runtime; build; native; contentfiles; analyzers; buildtransitive + + + + + + + + + + diff --git a/RobotApp/Services/Robot/Actions/RobotAction.cs b/RobotApp/Services/Robot/Actions/RobotAction.cs new file mode 100644 index 0000000..0b30496 --- /dev/null +++ b/RobotApp/Services/Robot/Actions/RobotAction.cs @@ -0,0 +1,191 @@ +using Grpc.Core; +using RobotApp.VDA5050.Factsheet; +using RobotApp.VDA5050.InstantAction; +using RobotApp.VDA5050.State; +using RobotApp.VDA5050.Type; + +namespace RobotApp.Services.Robot.Actions; + +public abstract class RobotAction(IServiceProvider serviceProvider) : IAsyncDisposable +{ + public ActionType Type { get; private set; } + public string Id { get; private set; } = ""; + public string Description { get; private set; } = ""; + public BlockingType BlockingType { get; private set; } + public ActionParameter[] Parameters { get; private set; } = []; + public ActionStatus Status { get; protected set; } = ActionStatus.WAITING; + public string ResultDescription { get; set; } = ""; + public bool IsCompleted => Status == ActionStatus.FINISHED || Status == ActionStatus.FAILED; + public ActionScopes ActionScope { get; set; } + + private WatchTimerAsync? ActionTimer; + private const int ActionInterval = 200; + + protected IServiceProvider ServiceProvider = serviceProvider; + protected VDA5050.InstantAction.Action? Action; + protected IServiceScope? Scope; + protected Logger? Logger; + protected AgvAction? AgvAction; + protected bool IsPaused = false; + protected ActionStatus HistoryStatus; + + public bool Initialize(ActionScopes actionScope, VDA5050.InstantAction.Action action) + { + Status = ActionStatus.WAITING; + ActionScope = actionScope; + Action = action; + return Initialize(); + } + + public void Start() + { + if (Status != ActionStatus.WAITING) return; + Scope ??= ServiceProvider.CreateScope(); + Logger = Scope.ServiceProvider.GetRequiredService>(); + ActionTimer = new(ActionInterval, ActionHandler, Logger); + Status = ActionStatus.INITIALIZING; + ActionTimer.Start(); + } + + public void Pause() + { + HistoryStatus = Status; + Status = ActionStatus.PAUSED; + } + + public void Resume() + { + if (Status == ActionStatus.PAUSED) Status = ActionStatus.WAITING; + } + + public void Cancel() + { + Dispose(); + } + + public async Task WaitAsync(CancellationToken cancellationToken) + { + while (!cancellationToken.IsCancellationRequested && !IsCompleted) + { + await Task.Delay(500, cancellationToken).ConfigureAwait(false); + } + } + + public void Wait(CancellationToken cancellationToken) + { + while (!cancellationToken.IsCancellationRequested && !IsCompleted) + { + Task.Delay(500, cancellationToken).Wait(cancellationToken); + } + } + + protected virtual Task StartAction() + { + return Task.CompletedTask; + } + + protected virtual Task ExecuteAction() + { + return Task.CompletedTask; + } + + protected virtual Task PauseAction() + { + return Task.CompletedTask; + } + + protected virtual Task ResumeAction() + { + return Task.CompletedTask; + } + + protected virtual bool Initialize() + { + if (Action is null) return false; + Scope ??= ServiceProvider.CreateScope(); + var FactsheetService = Scope.ServiceProvider.GetRequiredService(); + + if (Enum.TryParse(Action.ActionType, out ActionType type)) Type = type; + else return false; + + var actionStore = FactsheetService.GetAction(Type); + if (actionStore is null) return false; + + if (Enum.TryParse(Action.BlockingType, out BlockingType blockingType)) BlockingType = blockingType; + else return false; + + if (!actionStore.BlockingTypes.Any(bt => bt == BlockingType.ToString()) || !actionStore.ActionScopes.Any(sp => sp == ActionScope.ToString())) + { + return false; + } + + if (actionStore.ActionParameters.Length > 0) + { + foreach (var parameterStore in actionStore.ActionParameters) + { + if (!parameterStore.IsOptional) + { + if (!Action.ActionParameters.Any(a => a.Key == parameterStore.Key)) + { + return false; + } + } + } + Parameters = Action.ActionParameters; + } + + Id = Action.ActionId; + Description = Action.ActionDescription; + AgvAction = actionStore; + Status = ActionStatus.WAITING; + return true; + } + + private async Task ActionHandler() + { + try + { + if (Status == ActionStatus.INITIALIZING) + { + Logger?.Info($"Thực hiện action {Type}"); + Status = ActionStatus.RUNNING; + await StartAction(); + } + else if (Status == ActionStatus.RUNNING) + { + await ExecuteAction(); + } + else if (Status == ActionStatus.PAUSED) + { + await PauseAction(); + } + else if (Status == ActionStatus.WAITING && IsPaused) + { + await ResumeAction(); + } + + if (IsCompleted) + { + await Dispose(); + } + } + catch (Exception ex) + { + Logger?.Error($"Thực hiện action {Type} xảy ra lỗi: {ex.Message}"); + Status = ActionStatus.FAILED; + ResultDescription = $"Thực hiện action {Type} xảy ra lỗi: {ex.Message}"; + } + } + + private Task Dispose() + { + ActionTimer?.Dispose(); + return Task.CompletedTask; + } + + public async ValueTask DisposeAsync() + { + await Dispose(); + GC.SuppressFinalize(this); + } +} diff --git a/RobotApp/Services/Robot/Actions/RobotCancelOrderAction.cs b/RobotApp/Services/Robot/Actions/RobotCancelOrderAction.cs new file mode 100644 index 0000000..dd4ad5e --- /dev/null +++ b/RobotApp/Services/Robot/Actions/RobotCancelOrderAction.cs @@ -0,0 +1,14 @@ +namespace RobotApp.Services.Robot.Actions; + +public class RobotCancelOrderAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) +{ + protected override Task StartAction() + { + return base.StartAction(); + } + + protected override Task ExecuteAction() + { + return base.ExecuteAction(); + } +} diff --git a/RobotApp/Services/Robot/Actions/RobotDockToAction.cs b/RobotApp/Services/Robot/Actions/RobotDockToAction.cs new file mode 100644 index 0000000..d319905 --- /dev/null +++ b/RobotApp/Services/Robot/Actions/RobotDockToAction.cs @@ -0,0 +1,14 @@ +namespace RobotApp.Services.Robot.Actions; + +public class RobotDockToAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) +{ + protected override Task StartAction() + { + return base.StartAction(); + } + + protected override Task ExecuteAction() + { + return base.ExecuteAction(); + } +} diff --git a/RobotApp/Services/Robot/Actions/RobotFactsheetRequestAction.cs b/RobotApp/Services/Robot/Actions/RobotFactsheetRequestAction.cs new file mode 100644 index 0000000..c047144 --- /dev/null +++ b/RobotApp/Services/Robot/Actions/RobotFactsheetRequestAction.cs @@ -0,0 +1,14 @@ +namespace RobotApp.Services.Robot.Actions; + +public class RobotFactsheetRequestAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) +{ + protected override Task StartAction() + { + return base.StartAction(); + } + + protected override Task ExecuteAction() + { + return base.ExecuteAction(); + } +} diff --git a/RobotApp/Services/Robot/Actions/RobotInitPositionAction.cs b/RobotApp/Services/Robot/Actions/RobotInitPositionAction.cs new file mode 100644 index 0000000..10ec054 --- /dev/null +++ b/RobotApp/Services/Robot/Actions/RobotInitPositionAction.cs @@ -0,0 +1,14 @@ +namespace RobotApp.Services.Robot.Actions; + +public class RobotInitPositionAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) +{ + protected override Task StartAction() + { + return base.StartAction(); + } + + protected override Task ExecuteAction() + { + return base.ExecuteAction(); + } +} diff --git a/RobotApp/Services/Robot/Actions/RobotLiftDownAction.cs b/RobotApp/Services/Robot/Actions/RobotLiftDownAction.cs new file mode 100644 index 0000000..e142a41 --- /dev/null +++ b/RobotApp/Services/Robot/Actions/RobotLiftDownAction.cs @@ -0,0 +1,14 @@ +namespace RobotApp.Services.Robot.Actions; + +public class RobotLiftDownAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) +{ + protected override Task StartAction() + { + return base.StartAction(); + } + + protected override Task ExecuteAction() + { + return base.ExecuteAction(); + } +} diff --git a/RobotApp/Services/Robot/Actions/RobotLiftRotateAction.cs b/RobotApp/Services/Robot/Actions/RobotLiftRotateAction.cs new file mode 100644 index 0000000..2b4e480 --- /dev/null +++ b/RobotApp/Services/Robot/Actions/RobotLiftRotateAction.cs @@ -0,0 +1,14 @@ +namespace RobotApp.Services.Robot.Actions; + +public class RobotLiftRotateAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) +{ + protected override Task StartAction() + { + return base.StartAction(); + } + + protected override Task ExecuteAction() + { + return base.ExecuteAction(); + } +} diff --git a/RobotApp/Services/Robot/Actions/RobotLiftUpAction.cs b/RobotApp/Services/Robot/Actions/RobotLiftUpAction.cs new file mode 100644 index 0000000..6e4e796 --- /dev/null +++ b/RobotApp/Services/Robot/Actions/RobotLiftUpAction.cs @@ -0,0 +1,14 @@ +namespace RobotApp.Services.Robot.Actions; + +public class RobotLiftUpAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) +{ + protected override Task StartAction() + { + return base.StartAction(); + } + + protected override Task ExecuteAction() + { + return base.ExecuteAction(); + } +} diff --git a/RobotApp/Services/Robot/Actions/RobotMoveStraightToCoorAction.cs b/RobotApp/Services/Robot/Actions/RobotMoveStraightToCoorAction.cs new file mode 100644 index 0000000..1d93485 --- /dev/null +++ b/RobotApp/Services/Robot/Actions/RobotMoveStraightToCoorAction.cs @@ -0,0 +1,14 @@ +namespace RobotApp.Services.Robot.Actions; + +public class RobotMoveStraightToCoorAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) +{ + protected override Task StartAction() + { + return base.StartAction(); + } + + protected override Task ExecuteAction() + { + return base.ExecuteAction(); + } +} diff --git a/RobotApp/Services/Robot/Actions/RobotMoveStraightWithDistanceAction.cs b/RobotApp/Services/Robot/Actions/RobotMoveStraightWithDistanceAction.cs new file mode 100644 index 0000000..eda820f --- /dev/null +++ b/RobotApp/Services/Robot/Actions/RobotMoveStraightWithDistanceAction.cs @@ -0,0 +1,14 @@ +namespace RobotApp.Services.Robot.Actions; + +public class RobotMoveStraightWithDistanceAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) +{ + protected override Task StartAction() + { + return base.StartAction(); + } + + protected override Task ExecuteAction() + { + return base.ExecuteAction(); + } +} diff --git a/RobotApp/Services/Robot/Actions/RobotMutedBaseOffAction.cs b/RobotApp/Services/Robot/Actions/RobotMutedBaseOffAction.cs new file mode 100644 index 0000000..871ee29 --- /dev/null +++ b/RobotApp/Services/Robot/Actions/RobotMutedBaseOffAction.cs @@ -0,0 +1,14 @@ +namespace RobotApp.Services.Robot.Actions; + +public class RobotMutedBaseOffAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) +{ + protected override Task StartAction() + { + return base.StartAction(); + } + + protected override Task ExecuteAction() + { + return base.ExecuteAction(); + } +} diff --git a/RobotApp/Services/Robot/Actions/RobotMutedBaseOnAction.cs b/RobotApp/Services/Robot/Actions/RobotMutedBaseOnAction.cs new file mode 100644 index 0000000..e24b9f1 --- /dev/null +++ b/RobotApp/Services/Robot/Actions/RobotMutedBaseOnAction.cs @@ -0,0 +1,14 @@ +namespace RobotApp.Services.Robot.Actions; + +public class RobotMutedBaseOnAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) +{ + protected override Task StartAction() + { + return base.StartAction(); + } + + protected override Task ExecuteAction() + { + return base.ExecuteAction(); + } +} diff --git a/RobotApp/Services/Robot/Actions/RobotMutedLoadOffAction.cs b/RobotApp/Services/Robot/Actions/RobotMutedLoadOffAction.cs new file mode 100644 index 0000000..4726427 --- /dev/null +++ b/RobotApp/Services/Robot/Actions/RobotMutedLoadOffAction.cs @@ -0,0 +1,14 @@ +namespace RobotApp.Services.Robot.Actions; + +public class RobotMutedLoadOffAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) +{ + protected override Task StartAction() + { + return base.StartAction(); + } + + protected override Task ExecuteAction() + { + return base.ExecuteAction(); + } +} diff --git a/RobotApp/Services/Robot/Actions/RobotMutedLoadOnAction.cs b/RobotApp/Services/Robot/Actions/RobotMutedLoadOnAction.cs new file mode 100644 index 0000000..82b2c8b --- /dev/null +++ b/RobotApp/Services/Robot/Actions/RobotMutedLoadOnAction.cs @@ -0,0 +1,14 @@ +namespace RobotApp.Services.Robot.Actions; + +public class RobotMutedLoadOnAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) +{ + protected override Task StartAction() + { + return base.StartAction(); + } + + protected override Task ExecuteAction() + { + return base.ExecuteAction(); + } +} diff --git a/RobotApp/Services/Robot/Actions/RobotRotateAction.cs b/RobotApp/Services/Robot/Actions/RobotRotateAction.cs new file mode 100644 index 0000000..ca9bce8 --- /dev/null +++ b/RobotApp/Services/Robot/Actions/RobotRotateAction.cs @@ -0,0 +1,14 @@ +namespace RobotApp.Services.Robot.Actions; + +public class RobotRotateAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) +{ + protected override Task StartAction() + { + return base.StartAction(); + } + + protected override Task ExecuteAction() + { + return base.ExecuteAction(); + } +} diff --git a/RobotApp/Services/Robot/Actions/RobotRotateKeepLift.cs b/RobotApp/Services/Robot/Actions/RobotRotateKeepLift.cs new file mode 100644 index 0000000..b93d8ec --- /dev/null +++ b/RobotApp/Services/Robot/Actions/RobotRotateKeepLift.cs @@ -0,0 +1,14 @@ +namespace RobotApp.Services.Robot.Actions; + +public class RobotRotateKeepLift(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) +{ + protected override Task StartAction() + { + return base.StartAction(); + } + + protected override Task ExecuteAction() + { + return base.ExecuteAction(); + } +} diff --git a/RobotApp/Services/Robot/Actions/RobotStartChargingAction.cs b/RobotApp/Services/Robot/Actions/RobotStartChargingAction.cs new file mode 100644 index 0000000..8b054d1 --- /dev/null +++ b/RobotApp/Services/Robot/Actions/RobotStartChargingAction.cs @@ -0,0 +1,14 @@ +namespace RobotApp.Services.Robot.Actions; + +public class RobotStartChargingAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) +{ + protected override Task StartAction() + { + return base.StartAction(); + } + + protected override Task ExecuteAction() + { + return base.ExecuteAction(); + } +} diff --git a/RobotApp/Services/Robot/Actions/RobotStartPauseAction.cs b/RobotApp/Services/Robot/Actions/RobotStartPauseAction.cs new file mode 100644 index 0000000..1dded4f --- /dev/null +++ b/RobotApp/Services/Robot/Actions/RobotStartPauseAction.cs @@ -0,0 +1,23 @@ +using RobotApp.VDA5050.State; + +namespace RobotApp.Services.Robot.Actions; + +public class RobotStartPauseAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) +{ + protected override Task StartAction() + { + Scope ??= ServiceProvider.CreateScope(); + var robotController = Scope.ServiceProvider.GetRequiredService(); + robotController.Pause(); + Status = ActionStatus.FINISHED; + ResultDescription = AgvAction is not null ? AgvAction.ResultDescription : ResultDescription; + return base.StartAction(); + } + + protected override Task ExecuteAction() + { + Status = ActionStatus.FINISHED; + ResultDescription = AgvAction is not null ? AgvAction.ResultDescription : ResultDescription; + return base.ExecuteAction(); + } +} diff --git a/RobotApp/Services/Robot/Actions/RobotStateRequestAction.cs b/RobotApp/Services/Robot/Actions/RobotStateRequestAction.cs new file mode 100644 index 0000000..b36f8c7 --- /dev/null +++ b/RobotApp/Services/Robot/Actions/RobotStateRequestAction.cs @@ -0,0 +1,14 @@ +namespace RobotApp.Services.Robot.Actions; + +public class RobotStateRequestAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) +{ + protected override Task StartAction() + { + return base.StartAction(); + } + + protected override Task ExecuteAction() + { + return base.ExecuteAction(); + } +} diff --git a/RobotApp/Services/Robot/Actions/RobotStopChargingAction.cs b/RobotApp/Services/Robot/Actions/RobotStopChargingAction.cs new file mode 100644 index 0000000..0f07d13 --- /dev/null +++ b/RobotApp/Services/Robot/Actions/RobotStopChargingAction.cs @@ -0,0 +1,14 @@ +namespace RobotApp.Services.Robot.Actions; + +public class RobotStopChargingAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) +{ + protected override Task StartAction() + { + return base.StartAction(); + } + + protected override Task ExecuteAction() + { + return base.ExecuteAction(); + } +} diff --git a/RobotApp/Services/Robot/Actions/RobotStopPauseAction.cs b/RobotApp/Services/Robot/Actions/RobotStopPauseAction.cs new file mode 100644 index 0000000..49f1e0c --- /dev/null +++ b/RobotApp/Services/Robot/Actions/RobotStopPauseAction.cs @@ -0,0 +1,14 @@ +namespace RobotApp.Services.Robot.Actions; + +public class RobotStopPauseAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider) +{ + protected override Task StartAction() + { + return base.StartAction(); + } + + protected override Task ExecuteAction() + { + return base.ExecuteAction(); + } +} diff --git a/RobotApp/Services/Robot/RobotAction.cs b/RobotApp/Services/Robot/RobotAction.cs deleted file mode 100644 index 02c9893..0000000 --- a/RobotApp/Services/Robot/RobotAction.cs +++ /dev/null @@ -1,31 +0,0 @@ -using RobotApp.Interfaces; -using RobotApp.VDA5050.State; - -namespace RobotApp.Services.Robot; - -public class RobotAction : IInstantActions -{ - public ActionState[] ActionStates { get; private set; } = []; - - public bool HasActionRunning => throw new NotImplementedException(); - - public bool AddInstanceAction(VDA5050.InstantAction.Action action) - { - throw new NotImplementedException(); - } - - public bool AddOrderActions(VDA5050.InstantAction.Action[] actions) - { - throw new NotImplementedException(); - } - - public bool StartAction(string actionId) - { - throw new NotImplementedException(); - } - - public bool StopAction() - { - throw new NotImplementedException(); - } -} diff --git a/RobotApp/Services/Robot/RobotActionController.cs b/RobotApp/Services/Robot/RobotActionController.cs new file mode 100644 index 0000000..e9cb321 --- /dev/null +++ b/RobotApp/Services/Robot/RobotActionController.cs @@ -0,0 +1,118 @@ +using RobotApp.Interfaces; +using RobotApp.Services.Robot.Actions; +using RobotApp.VDA5050.Factsheet; +using RobotApp.VDA5050.State; +using RobotApp.VDA5050.Type; +using System.Collections.Concurrent; + +namespace RobotApp.Services.Robot; + +public class RobotActionController(Logger Logger, RobotActionStorage ActionStorage) : BackgroundService, IInstantActions +{ + public ActionState[] ActionStates => [.. Actions.Values.Select(a => new ActionState + { + ActionId = a.Id, + ActionType = a.Type.ToString(), + ActionDescription = a.Description, + ActionStatus = a.Status.ToString(), + ResultDescription = a.ResultDescription, + })]; + public bool HasActionRunning => !ActionQueue.IsEmpty || Actions.Values.Any(a => !a.IsCompleted); + + private readonly ConcurrentDictionary Actions = []; + private readonly ConcurrentQueue<(ActionScopes scope, VDA5050.InstantAction.Action action)> ActionQueue = []; + + private WatchTimer? HandlerTimer; + private const int HandlerInterval = 200; + + public void AddInstantAction(VDA5050.InstantAction.Action action) + { + ActionQueue.Enqueue((ActionScopes.INSTANT, action)); + } + + public void AddOrderActions(VDA5050.InstantAction.Action[] actions) + { + foreach (var action in actions) + { + ActionQueue.Enqueue((ActionScopes.NODE, action)); + } + } + + public void StartOrderAction(string actionId, bool wait) + { + if (Actions.TryGetValue(actionId, out RobotAction? robotAction) && robotAction is not null) + { + robotAction.Start(); + if(wait) + { + robotAction.Wait(CancellationToken.None); + } + } + } + + public void PauseActions() + { + foreach(var action in Actions.Values) + { + action.Pause(); + } + } + + public void ResumeActions() + { + foreach (var action in Actions.Values) + { + action.Resume(); + } + } + + private void ActionHandler() + { + try + { + while(!ActionQueue.IsEmpty) + { + if (ActionQueue.TryDequeue(out var result)) + { + if (Actions.ContainsKey(result.action.ActionId)) return; + if (Enum.TryParse(result.action.ActionType, out ActionType actionType)) + { + var robotAction = ActionStorage[actionType]; + if (robotAction is not null) + { + var init = robotAction.Initialize(result.scope, result.action); + if (init) + { + Actions.TryAdd(result.action.ActionId, robotAction); + if(result.scope == ActionScopes.INSTANT) robotAction.Start(); + } + } + } + } + } + } + catch (Exception ex) + { + Logger?.Error($"Khởi tạo action xảy ra lỗi: {ex.Message}"); + } + } + + public void ClearInstantActions() + { + ActionQueue.Clear(); + Actions.Clear(); + } + + protected override async Task ExecuteAsync(CancellationToken stoppingToken) + { + await Task.Yield(); + HandlerTimer = new(HandlerInterval, ActionHandler, Logger); + HandlerTimer.Start(); + } + + public override Task StopAsync(CancellationToken cancellationToken) + { + HandlerTimer?.Dispose(); + return base.StopAsync(cancellationToken); + } +} diff --git a/RobotApp/Services/Robot/RobotActionStorage.cs b/RobotApp/Services/Robot/RobotActionStorage.cs new file mode 100644 index 0000000..20966f4 --- /dev/null +++ b/RobotApp/Services/Robot/RobotActionStorage.cs @@ -0,0 +1,33 @@ +using RobotApp.Services.Robot.Actions; +using RobotApp.VDA5050.Type; + +namespace RobotApp.Services.Robot; + + +public class RobotActionStorage(IServiceProvider ServiceProvider) +{ + public RobotAction? this[ActionType type] => type switch + { + ActionType.startPause => new RobotStartPauseAction(ServiceProvider), + ActionType.stopPause => new RobotStopPauseAction(ServiceProvider), + ActionType.startCharging => new RobotStartChargingAction(ServiceProvider), + ActionType.stopCharging => new RobotStopChargingAction(ServiceProvider), + ActionType.initPosition => new RobotInitPositionAction(ServiceProvider), + ActionType.stateRequest => new RobotStateRequestAction(ServiceProvider), + ActionType.factsheetRequest => new RobotFactsheetRequestAction(ServiceProvider), + ActionType.cancelOrder => new RobotCancelOrderAction(ServiceProvider), + ActionType.liftUp => new RobotLiftUpAction(ServiceProvider), + ActionType.liftDown => new RobotLiftDownAction(ServiceProvider), + ActionType.liftRotate => new RobotLiftRotateAction(ServiceProvider), + ActionType.rotate => new RobotRotateAction(ServiceProvider), + ActionType.rotateKeepLift => new RobotRotateKeepLift(ServiceProvider), + ActionType.mutedBaseOn => new RobotMutedBaseOnAction(ServiceProvider), + ActionType.mutedBaseOff => new RobotMutedBaseOffAction(ServiceProvider), + ActionType.mutedLoadOn => new RobotMutedLoadOnAction(ServiceProvider), + ActionType.mutedLoadOff => new RobotMutedLoadOffAction(ServiceProvider), + ActionType.dockTo => new RobotDockToAction(ServiceProvider), + ActionType.moveStraightToCoor => new RobotMoveStraightToCoorAction(ServiceProvider), + ActionType.moveStraightWithDistance => new RobotMoveStraightWithDistanceAction(ServiceProvider), + _ => null, + }; +} diff --git a/RobotApp/Services/Robot/RobotConfiguration.cs b/RobotApp/Services/Robot/RobotConfiguration.cs index 54a7c22..659af94 100644 --- a/RobotApp/Services/Robot/RobotConfiguration.cs +++ b/RobotApp/Services/Robot/RobotConfiguration.cs @@ -22,4 +22,6 @@ public class RobotConfiguration public byte PLCUnitId { get; set; } = 1; public bool IsSimulation { get; set; } = true; public SimulationModel SimulationModel { get; set; } = new(); + + public string XlocAddress { get; set; } = ""; } diff --git a/RobotApp/Services/Robot/RobotConnection.cs b/RobotApp/Services/Robot/RobotConnection.cs index ce97d0b..bede774 100644 --- a/RobotApp/Services/Robot/RobotConnection.cs +++ b/RobotApp/Services/Robot/RobotConnection.cs @@ -54,7 +54,7 @@ public class RobotConnection(RobotConfiguration RobotConfiguration, public async Task StartConnection(CancellationToken cancellationToken) { - MqttClient = new MQTTClient(SerialNumber, VDA5050Setting, MQTTClientLogger); + MqttClient = new MQTTClient(RobotConfiguration.SerialNumber, VDA5050Setting, MQTTClientLogger); MqttClient.OrderChanged += OrderChanged; MqttClient.InstanceActionsChanged += InstanceActionsChanged; await MqttClient.ConnectAsync(cancellationToken); diff --git a/RobotApp/Services/Robot/RobotController.cs b/RobotApp/Services/Robot/RobotController.cs index 6a01110..69256fd 100644 --- a/RobotApp/Services/Robot/RobotController.cs +++ b/RobotApp/Services/Robot/RobotController.cs @@ -26,10 +26,10 @@ public partial class RobotController(IOrder OrderManager, await InitializationingHandler(stoppingToken); } - public override async Task StopAsync(CancellationToken cancellationToken) + public override Task StopAsync(CancellationToken cancellationToken) { StopHandler(); - await base.StopAsync(cancellationToken); + return base.StopAsync(cancellationToken); } public void NewOrderUpdated(OrderMsg order) @@ -90,4 +90,14 @@ public partial class RobotController(IOrder OrderManager, } } } + + public void Pause() + { + + } + + public void Resume() + { + + } } diff --git a/RobotApp/Services/Robot/RobotFactsheet.cs b/RobotApp/Services/Robot/RobotFactsheet.cs index 7168631..1210f57 100644 --- a/RobotApp/Services/Robot/RobotFactsheet.cs +++ b/RobotApp/Services/Robot/RobotFactsheet.cs @@ -1,34 +1,348 @@ - -using RobotApp.Common.Shares; +using RobotApp.Common.Shares; using RobotApp.VDA5050; using RobotApp.VDA5050.Factsheet; +using RobotApp.VDA5050.InstantAction; +using RobotApp.VDA5050.Type; using System.Text.Json; namespace RobotApp.Services.Robot; -public class RobotFactsheet(RobotConnection RobotConnection) : BackgroundService + +public class RobotFactsheet(RobotConnection RobotConnection, RobotConfiguration RobotConfiguration) : BackgroundService { + private readonly Dictionary AgvActions = new() + { + { ActionType.startPause, StartPause}, + { ActionType.stopPause, StopPause}, + { ActionType.startCharging, StartCharging}, + { ActionType.stopCharging, StopCharging}, + { ActionType.initPosition, InitPosition}, + { ActionType.stateRequest, StateRequest}, + { ActionType.factsheetRequest, FactsheetRequest}, + { ActionType.cancelOrder, CancelOrder}, + { ActionType.liftUp, LiftUp}, + { ActionType.liftDown, LiftDown}, + { ActionType.liftRotate, LiftRotate}, + { ActionType.rotate, Rotate}, + { ActionType.rotateKeepLift, RotateKeepLift}, + { ActionType.mutedBaseOn, MutedBaseOn}, + { ActionType.mutedBaseOff, MutedBaseOff}, + { ActionType.mutedLoadOn, MutedLoadOn}, + { ActionType.mutedLoadOff, MutedLoadOff}, + { ActionType.dockTo, DockTo}, + { ActionType.moveStraightToCoor, MoveStraightToCoor}, + { ActionType.moveStraightWithDistance, MoveStraightWithDistance}, + }; + + public AgvAction? GetAction(ActionType actionType) => AgvActions.TryGetValue(actionType, out AgvAction? value) && value is not null ? value : null; + public async Task PubFactsheet() { if (!RobotConnection.IsConnected) return; FactSheetMsg factSheet = new() { - SerialNumber = RobotConnection.SerialNumber, + SerialNumber = RobotConfiguration.SerialNumber, ProtocolFeatures = new() { - AgvActions = [], + AgvActions = [..AgvActions.Values], } }; string factSheetJson = JsonSerializer.Serialize(factSheet, JsonOptionExtends.Write); await RobotConnection.Publish(VDA5050Topic.FACTSHEET.ToTopicString(), factSheetJson); } + protected override async Task ExecuteAsync(CancellationToken stoppingToken) { - while(!stoppingToken.IsCancellationRequested) + await Task.Yield(); + while (!stoppingToken.IsCancellationRequested) { - await Task.Delay(1000, stoppingToken); if (RobotConnection.IsConnected) break; + await Task.Delay(1000); } await PubFactsheet(); } + + public readonly static AgvAction StartPause = new() + { + ActionType = ActionType.startPause.ToString(), + ActionDescription = "Tam dừng robot.", + ActionScopes = [ActionScopes.INSTANT.ToString()], + ActionParameters = [], + ResultDescription = "Robot đã tạm dừng.", + BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString()], + }; + + public readonly static AgvAction StopPause = new() + { + ActionType = ActionType.stopPause.ToString(), + ActionDescription = "Tiếp tục hoạt động robot sau khi tạm dừng.", + ActionScopes = [ActionScopes.INSTANT.ToString()], + ActionParameters = [], + ResultDescription = "Robot đã tiếp tục hoạt động.", + BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString(), BlockingType.HARD.ToString()], + }; + + public readonly static AgvAction StartCharging = new() + { + ActionType = ActionType.startCharging.ToString(), + ActionDescription = "Bắt đầu quá trình sạc pin.", + ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()], + ActionParameters = [], + ResultDescription = "Robot đã bắt đầu sạc pin.", + BlockingTypes = [BlockingType.HARD.ToString()], + }; + + public readonly static AgvAction StopCharging = new() + { + ActionType = ActionType.stopCharging.ToString(), + ActionDescription = "Kết thúc quá trình sạc pin.", + ActionScopes = [ActionScopes.INSTANT.ToString()], + ActionParameters = [], + ResultDescription = "Robot đã kết thúc sạc pin.", + BlockingTypes = [BlockingType.HARD.ToString()], + }; + + public readonly static AgvAction InitPosition = new() + { + ActionType = ActionType.initPosition.ToString(), + ActionDescription = "Khởi tạo vị trí robot.", + ActionScopes = [ActionScopes.INSTANT.ToString()], + ActionParameters = [ + new() + { + Key = "x", + Description = "Tọa độ X của vị trí khởi tạo.", + ValueDataType = ValueDataType.FLOAT.ToString(), + IsOptional = false, + }, + new() + { + Key = "y", + Description = "Tọa độ Y của vị trí khởi tạo.", + ValueDataType = ValueDataType.FLOAT.ToString(), + IsOptional = false, + }, + new() + { + Key = "theta", + Description = "Góc quay (theta) của vị trí khởi tạo. (rad)", + ValueDataType = ValueDataType.FLOAT.ToString(), + IsOptional = false, + }], + ResultDescription = "Robot đã khởi tạo vị trí.", + BlockingTypes = [BlockingType.HARD.ToString()], + }; + + public readonly static AgvAction StateRequest = new() + { + ActionType = ActionType.stateRequest.ToString(), + ActionDescription = "Yêu cầu gửi trạng thái robot ngay lập tức.", + ActionScopes = [ActionScopes.INSTANT.ToString()], + ActionParameters = [], + ResultDescription = "Robot đã gửi trạng thái ngay lập tức.", + BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString()], + }; + + public readonly static AgvAction FactsheetRequest = new() + { + ActionType = ActionType.factsheetRequest.ToString(), + ActionDescription = "Yêu cầu gửi Factsheet robot ngay lập tức.", + ActionScopes = [ActionScopes.INSTANT.ToString()], + ActionParameters = [], + ResultDescription = "Robot đã gửi Factsheet ngay lập tức.", + BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString()], + }; + + public readonly static AgvAction CancelOrder = new() + { + ActionType = ActionType.cancelOrder.ToString(), + ActionDescription = "Hủy bỏ Order hiện tại của robot.", + ActionScopes = [ActionScopes.INSTANT.ToString()], + ActionParameters = [], + ResultDescription = "Robot đã hủy bỏ Order hiện tại.", + BlockingTypes = [BlockingType.NONE.ToString()], + }; + + public readonly static AgvAction LiftUp = new() + { + ActionType = ActionType.liftUp.ToString(), + ActionDescription = "Nâng cao bàn nâng của robot.", + ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()], + ActionParameters = [], + ResultDescription = "Robot đã nâng cao bàn nâng.", + BlockingTypes = [BlockingType.HARD.ToString()], + }; + + public readonly static AgvAction LiftDown = new() + { + ActionType = ActionType.liftDown.ToString(), + ActionDescription = "Hạ thấp bàn nâng của robot.", + ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()], + ActionParameters = [], + ResultDescription = "Robot đã hạ thấp bàn nâng.", + BlockingTypes = [BlockingType.HARD.ToString()], + }; + + public readonly static AgvAction LiftRotate = new() + { + ActionType = ActionType.liftRotate.ToString(), + ActionDescription = "Xoay bàn nâng của robot.", + ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()], + ActionParameters = [ + new() + { + Key = "angle", + Description = "Góc xoay của bàn nâng. (rad)", + ValueDataType = ValueDataType.FLOAT.ToString(), + IsOptional = false, + }], + ResultDescription = "Robot đã xoay bàn nâng.", + BlockingTypes = [BlockingType.HARD.ToString()], + }; + + public readonly static AgvAction Rotate = new() + { + ActionType = ActionType.rotate.ToString(), + ActionDescription = "Xoay robot tại chỗ.", + ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()], + ActionParameters = [ + new() + { + Key = "angle", + Description = "Góc xoay của robot. (rad)", + ValueDataType = ValueDataType.FLOAT.ToString(), + IsOptional = false, + }], + ResultDescription = "Robot đã xoay tại chỗ.", + BlockingTypes = [BlockingType.HARD.ToString()], + }; + + public readonly static AgvAction RotateKeepLift = new() + { + ActionType = ActionType.rotateKeepLift.ToString(), + ActionDescription = "Xoay robot tại chỗ giữ nguyên trạng thái bàn nâng.", + ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()], + ActionParameters = [ + new() + { + Key = "angle", + Description = "Góc xoay của robot. (rad)", + ValueDataType = ValueDataType.FLOAT.ToString(), + IsOptional = false, + }], + ResultDescription = "Robot đã xoay tại chỗ giữ nguyên trạng thái bàn nâng.", + BlockingTypes = [BlockingType.HARD.ToString()], + }; + + public readonly static AgvAction MutedBaseOn = new() + { + ActionType = ActionType.mutedBaseOn.ToString(), + ActionDescription = "Bật chế độ muted base robot.", + ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()], + ActionParameters = [], + ResultDescription = "Robot đã bật chế độ muted base.", + BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString()], + }; + + public readonly static AgvAction MutedBaseOff = new() + { + ActionType = ActionType.mutedBaseOff.ToString(), + ActionDescription = "Tắt chế độ muted base robot.", + ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()], + ActionParameters = [], + ResultDescription = "Robot đã tắt chế độ muted base.", + BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString()], + }; + + public readonly static AgvAction MutedLoadOn = new() + { + ActionType = ActionType.mutedLoadOn.ToString(), + ActionDescription = "Bật chế độ muted load robot.", + ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()], + ActionParameters = [], + ResultDescription = "Robot đã bật chế độ muted load.", + BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString()], + }; + + public readonly static AgvAction MutedLoadOff = new() + { + ActionType = ActionType.mutedLoadOff.ToString(), + ActionDescription = "Tắt chế độ muted load robot.", + ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()], + ActionParameters = [], + ResultDescription = "Robot đã tắt chế độ muted load.", + BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString()], + }; + + public readonly static AgvAction DockTo = new() + { + ActionType = ActionType.dockTo.ToString(), + ActionDescription = "Robot di chuyển vào vị trí đặc biết", + ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()], + ActionParameters = [ + new() + { + Key = "dockId", + Description = "ID của vị trí dock.", + ValueDataType = ValueDataType.STRING.ToString(), + IsOptional = false, + }], + ResultDescription = "Robot đã dock đến vị trí sạc hoặc bến đỗ.", + BlockingTypes = [BlockingType.HARD.ToString()], + }; + + public readonly static AgvAction MoveStraightToCoor = new() + { + ActionType = ActionType.moveStraightToCoor.ToString(), + ActionDescription = "Di chuyển thẳng đến tọa độ xác định.", + ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()], + ActionParameters = [ + new() + { + Key = "x", + Description = "Tọa độ X đích đến.", + ValueDataType = ValueDataType.FLOAT.ToString(), + IsOptional = false, + }, + new() + { + Key = "y", + Description = "Tọa độ Y đích đến.", + ValueDataType = ValueDataType.FLOAT.ToString(), + IsOptional = false, + }], + ResultDescription = "Robot đã di chuyển thẳng đến tọa độ xác định.", + BlockingTypes = [BlockingType.HARD.ToString()], + }; + + public readonly static AgvAction MoveStraightWithDistance = new() + { + ActionType = ActionType.moveStraightWithDistance.ToString(), + ActionDescription = "Di chuyển thẳng với khoảng cách xác định.", + ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()], + ActionParameters = [ + new() + { + Key = "distance", + Description = "Khoảng cách di chuyển. (m)", + ValueDataType = ValueDataType.FLOAT.ToString(), + IsOptional = false, + }, + new() + { + Key = "direction", + Description = "Hướng di chuyển: 1 - tiến, -1 - lùi.", + ValueDataType = ValueDataType.INTEGER.ToString(), + IsOptional = false, + }, + new() + { + Key = "angle", + Description = "Góc di chuyển so với hướng hiện tại của robot. (rad)", + ValueDataType = ValueDataType.FLOAT.ToString(), + IsOptional = true, + }], + ResultDescription = "Robot đã di chuyển thẳng với khoảng cách xác định.", + BlockingTypes = [BlockingType.HARD.ToString()], + }; } diff --git a/RobotApp/Services/Robot/RobotLocalization.cs b/RobotApp/Services/Robot/RobotLocalization.cs index 7d5c30f..2926cd0 100644 --- a/RobotApp/Services/Robot/RobotLocalization.cs +++ b/RobotApp/Services/Robot/RobotLocalization.cs @@ -1,83 +1,438 @@ -using RobotApp.Common.Shares; +using Google.Protobuf.WellKnownTypes; +using Grpc.Core; +using Grpc.Net.Client; +using RobotApp.Common.Shares; using RobotApp.Interfaces; using RobotApp.Services.Robot.Simulation; +using System; +using Xloc; +using static Xloc.XlocDiagnostics.Types; namespace RobotApp.Services.Robot; -public class Xloc +public class XlocData { + public UInt32 Sequence { get; set; } + public Timestamp Stamp { get; set; } = new(); + public string FrameId { get; set; } = ""; public double X { get; set; } public double Y { get; set; } public double Theta { get; set; } - public string SlamState { get; set; } = string.Empty; + public SlamState SlamState { get; set; } public string SlamStateDetail { get; set; } = string.Empty; public string CurrentActiveMap { get; set; } = string.Empty; public double Reliability { get; set; } public double MatchingScore { get; set; } - public bool IsReady { get; set; } + public bool IsReady { get; set; } = false; } -public class RobotLocalization(RobotConfiguration RobotConfiguration, SimulationVisualization SimVisualization) : ILocalization +public class RobotLocalization(RobotConfiguration RobotConfiguration, SimulationVisualization SimVisualization, Logger Logger) : BackgroundService, ILocalization { - public double X => IsSimulation ? SimVisualization.X : Xloc.X; + public UInt32 Sequence => IsSimulation ? 0 : XlocData.Sequence; + public Timestamp Stamp => IsSimulation ? new() : XlocData.Stamp; + public string FrameId => IsSimulation ? "" : XlocData.FrameId; + public double X => IsSimulation ? SimVisualization.X : XlocData.X; + public double Y => IsSimulation ? SimVisualization.Y : XlocData.Y; + public double Theta => IsSimulation ? SimVisualization.Theta * Math.PI / 180 : XlocData.Theta; + public string SlamState => IsSimulation ? "Localization" : XlocData.SlamState.ToString(); + public string SlamStateDetail => IsSimulation ? "" : XlocData.SlamStateDetail; + public string CurrentActiveMap => IsSimulation ? "" : XlocData.CurrentActiveMap; + public double Reliability => IsSimulation ? 100 : XlocData.Reliability; + public double MatchingScore => IsSimulation ? 100 : XlocData.MatchingScore; + public bool IsReady => IsSimulation || XlocData.IsReady; - public double Y => IsSimulation ? SimVisualization.Y : Xloc.Y; - - public double Theta => IsSimulation ? SimVisualization.Theta * Math.PI / 180 : Xloc.Theta; - - public string SlamState => IsSimulation ? "Localization" : Xloc.SlamState; - - public string SlamStateDetail => IsSimulation ? "" : Xloc.SlamStateDetail; - - public string CurrentActiveMap => IsSimulation ? "" : Xloc.CurrentActiveMap; - - public double Reliability => IsSimulation ? 100 : Xloc.Reliability; - - public double MatchingScore => IsSimulation ? 100 : Xloc.MatchingScore; - - public bool IsReady => IsSimulation || Xloc.IsReady; - - - private readonly Xloc Xloc = new(); + private readonly XlocData XlocData = new(); private readonly bool IsSimulation = RobotConfiguration.IsSimulation; + private readonly XlocServices.XlocServicesClient XlocClient = new(GrpcChannel.ForAddress(RobotConfiguration.XlocAddress)); + private WatchTimerAsync? ReaderTimer; + private const int ReaderInterval = 50; + private int TimerCounter = 0; + + private AsyncServerStreamingCall? XlocPose; + private AsyncServerStreamingCall? XlocDiagnostics; + public MessageResult ActivateMap(string mapName) { - throw new NotImplementedException(); + try + { + var response = XlocClient.ActivateMap(new ActivateMapRequest() + { + MapFileName = mapName + }); + if (response.Status.Code == StatusResponse.Types.StatusCode.Ok) return new(true); + else + { + Logger.Warning($"Kích hoạt bản đồ '{mapName}' thất bại. Kết quả trả về: {response.Status.Code} - {response.Status.Message}"); + return new(false, $"Kích hoạt bản đồ '{mapName}' thất bại"); + } + } + catch (Exception ex) + { + Logger.Warning($"Kích hoạt bản đồ '{mapName}' thất bại: {ex.Message}"); + return new(false, $"Kích hoạt bản đồ '{mapName}' thất bại"); + } } - public MessageResult ChangeMapOrigin(double x, double y, double theta) + public MessageResult SwitchMap(string mapName, bool useInitialPose, double x, double y, double theta) { - throw new NotImplementedException(); + try + { + var xyzw = QuaternionToXYZW(0, 0, theta); + var response = XlocClient.SwitchMap(new SwitchMapRequest() + { + MapFileName = mapName, + UseInitialPose = useInitialPose, + InitialPose = new Pose() + { + Position = new Point() + { + X = x, + Y = y, + Z = 0, + }, + Orientation = new Quaternion() + { + X = xyzw.x, + Y = xyzw.y, + Z = xyzw.z, + W = xyzw.z + } + } + }); + if (response.Status.Code == StatusResponse.Types.StatusCode.Ok) return new(true); + else + { + Logger.Warning($"Chuyển đổi bản đồ '{mapName}' thất bại. Kết quả trả về: {response.Status.Code} - {response.Status.Message}"); + return new(false, $"Chuyển đổi bản đồ '{mapName}' thất bại"); + } + } + catch (Exception ex) + { + Logger.Warning($"Chuyển đổi bản đồ '{mapName}' thất bại: {ex.Message}"); + return new(false, $"Chuyển đổi bản đồ '{mapName}' thất bại"); + } } - public MessageResult SetInitializePosition(double x, double y, double theta) + public MessageResult StartMapping() { - throw new NotImplementedException(); - } - - public MessageResult StartLocalization() - { - throw new NotImplementedException(); - } - - public MessageResult StartMapping(double resolution) - { - throw new NotImplementedException(); - } - - public MessageResult StopLocalization() - { - throw new NotImplementedException(); + try + { + var response = XlocClient.StartMapping(new()); + if (response.Status.Code == StatusResponse.Types.StatusCode.Ok) return new(true); + else + { + Logger.Warning($"Bắt đầu quá trình Mapping thất bại. Kết quả trả về: {response.Status.Code} - {response.Status.Message}"); + return new(false, "Bắt đầu quá trình Mapping thất bại"); + } + } + catch (Exception ex) + { + Logger.Warning($"Bắt đầu quá trình Mapping thất bại: {ex.Message}"); + return new(false, $"Bắt đầu quá trình Mapping thất bại"); + } } public MessageResult StopMapping(string mapName) { - throw new NotImplementedException(); + try + { + var response = XlocClient.StopMapping(new StopMappingRequest() + { + SaveMapFilename = mapName, + }); + if (response.Status.Code == StatusResponse.Types.StatusCode.Ok) return new(true); + else + { + Logger.Warning($"Kết thúc quá trình Mapping với tên '{mapName}' thất bại. Kết quả trả về: {response.Status.Code} - {response.Status.Message}"); + return new(false, $"Kết thúc quá trình Mapping với tên '{mapName}' thất bại."); + } + } + catch (Exception ex) + { + Logger.Warning($"Kết thúc quá trình Mapping với tên '{mapName}' thất bại: {ex.Message}"); + return new(false, $"Kết thúc quá trình Mapping với tên '{mapName}' thất bại."); + } } - public MessageResult SwitchMap(string mapName, double x, double y, double theta) + public MessageResult StartLocalization() { - throw new NotImplementedException(); + try + { + var response = XlocClient.StartLocalization(new()); + if (response.Status.Code == StatusResponse.Types.StatusCode.Ok) return new(true); + else + { + Logger.Warning($"Bắt đầu quá trình Localization thất bại. Kết quả trả về: {response.Status.Code} - {response.Status.Message}"); + return new(false, "Bắt đầu quá trình Localization thất bại"); + } + } + catch (Exception ex) + { + Logger.Warning($"Bắt đầu quá trình Localization thất bại: {ex.Message}"); + return new(false, $"Bắt đầu quá trình Localization thất bại"); + } + } + + public MessageResult StopLocalization() + { + try + { + var response = XlocClient.StopLocalization(new()); + if (response.Status.Code == StatusResponse.Types.StatusCode.Ok) return new(true); + else + { + Logger.Warning($"Kết thúc quá trình Localization thất bại. Kết quả trả về: {response.Status.Code} - {response.Status.Message}"); + return new(false, "Kết thúc quá trình Localization thất bại"); + } + } + catch (Exception ex) + { + Logger.Warning($"Kết thúc quá trình Localization thất bại: {ex.Message}"); + return new(false, $"Kết thúc quá trình Localization thất bại"); + } + } + + public MessageResult ChangeMapOrigin(double x, double y, double theta) + { + try + { + var xyzw = QuaternionToXYZW(0, 0, theta); + var response = XlocClient.ChangeMapOrigin(new ChangeMapOriginRequest() + { + NewMapOrigin = new Pose() + { + Position = new Point() + { + X = x, + Y = y, + Z = 0, + }, + Orientation = new Quaternion() + { + X = xyzw.x, + Y = xyzw.y, + Z = xyzw.z, + W = xyzw.w + } + } + }); + if (response.Status.Code == StatusResponse.Types.StatusCode.Ok) return new(true); + else + { + Logger.Warning($"Thay đổi gốc bản đồ thất bại. Kết quả trả về: {response.Status.Code} - {response.Status.Message}"); + return new(false, "Thay đổi gốc bản đồ thất bại"); + } + } + catch (Exception ex) + { + Logger.Warning($"Thay đổi gốc bản đồ thất bại: {ex.Message}"); + return new(false, $"Thay đổi gốc bản đồ thất bại"); + } + } + + public MessageResult StartUpdateMap() + { + try + { + var response = XlocClient.StartUpdateMap(new()); + if (response.Status.Code == StatusResponse.Types.StatusCode.Ok) return new(true); + else + { + Logger.Warning($"Bắt đầu quá trình cập nhật bản đồ thất bại. Kết quả trả về: {response.Status.Code} - {response.Status.Message}"); + return new(false, "Bắt đầu quá trình cập nhật bản đồ thất bại"); + } + } + catch (Exception ex) + { + Logger.Warning($"Bắt đầu quá trình cập nhật bản đồ thất bại: {ex.Message}"); + return new(false, $"Bắt đầu quá trình cập nhật bản đồ thất bại"); + } + } + + public MessageResult StopUpdateMap(bool save) + { + try + { + var response = XlocClient.StopUpdateMap(new StopUpdateMapRequest() + { + SaveUpdatedMap = save, + }); + if (response.Status.Code == StatusResponse.Types.StatusCode.Ok) return new(true); + else + { + Logger.Warning($"Kết thúc quá trình cập nhật bản đồ thất bại. Kết quả trả về: {response.Status.Code} - {response.Status.Message}"); + return new(false, "Kết thúc quá trình cập nhật bản đồ thất bại"); + } + } + catch (Exception ex) + { + Logger.Warning($"Kết thúc quá trình cập nhật bản đồ thất bại: {ex.Message}"); + return new(false, $"Kết thúc quá trình cập nhật bản đồ thất bại"); + } + } + + public MessageResult SetInitializePosition(double x, double y, double theta) + { + try + { + var xyzw = QuaternionToXYZW(0, 0, theta); + var response = XlocClient.SetInitialPose(new SetInitialPoseRequest() + { + InitialPose = new Pose() + { + Position = new Point() + { + X = x, + Y = y, + Z = 0, + }, + Orientation = new Quaternion() + { + X = xyzw.x, + Y = xyzw.y, + Z = xyzw.z, + W = xyzw.w + } + } + }); + if (response.Status.Code == StatusResponse.Types.StatusCode.Ok) return new(true); + else + { + Logger.Warning("Khởi tạo vị trí cho robot thất bại. Kết quả trả về: {response.Status.Code} - {response.Status.Message}"); + return new(false, "Khởi tạo vị trí cho robot thất bại"); + } + } + catch (Exception ex) + { + Logger.Warning($"Khởi tạo vị trí cho robot thất bại: {ex.Message}"); + return new(false, $"Khởi tạo vị trí cho robot thất bại"); + } + } + + public MessageResult ResetSlamError() + { + try + { + var response = XlocClient.ResetSlamError(new()); + if (response.Status.Code == StatusResponse.Types.StatusCode.Ok) return new(true); + else + { + Logger.Warning($"Xóa bỏ lỗi Slam thất bại. Kết quả trả về: {response.Status.Code} - {response.Status.Message}"); + return new(false, "Xóa bỏ lỗi Slam thất bại"); + } + } + catch (Exception ex) + { + Logger.Warning($"Xóa bỏ lỗi Slam thất bại: {ex.Message}"); + return new(false, $"Xóa bỏ lỗi Slam thất bại"); + } + } + + private static (double roll, double pitch, double yaw) QuaternionToRPY(double x, double y, double z, double w) + { + double sinr_cosp = 2.0 * (w * x + y * z); + double cosr_cosp = 1.0 - 2.0 * (x * x + y * y); + double roll = Math.Atan2(sinr_cosp, cosr_cosp); + + double sinp = 2.0 * (w * y - z * x); + double pitch; + if (sinp >= 1.0) pitch = Math.PI / 2.0; + else if (sinp <= -1.0) pitch = -Math.PI / 2.0; + else pitch = Math.Asin(sinp); + + double siny_cosp = 2.0 * (w * z + x * y); + double cosy_cosp = 1.0 - 2.0 * (y * y + z * z); + double yaw = Math.Atan2(siny_cosp, cosy_cosp); + return (roll, pitch, yaw); + } + + private static (double x, double y, double z, double w) QuaternionToXYZW(double roll, double pitch, double yaw) + { + double cy = Math.Cos(yaw * 0.5); + double sy = Math.Sin(yaw * 0.5); + double cp = Math.Cos(pitch * 0.5); + double sp = Math.Sin(pitch * 0.5); + double cr = Math.Cos(roll * 0.5); + double sr = Math.Sin(roll * 0.5); + + double w = cr * cp * cy + sr * sp * sy; + double x = sr * cp * cy - cr * sp * sy; + double y = cr * sp * cy + sr * cp * sy; + double z = cr * cp * sy - sr * sp * cy; + return (x, y, z, w); + } + + private async Task XlocReader() + { + try + { + if (XlocPose is null || !await XlocPose.ResponseStream.MoveNext()) + { + XlocPose = XlocClient.GetPose(new()); + return; + } + + XlocData.Sequence = XlocPose.ResponseStream.Current.Header.Seq; + XlocData.Stamp = XlocPose.ResponseStream.Current.Header.Stamp; + XlocData.FrameId = XlocPose.ResponseStream.Current.Header.FrameId; + XlocData.X = XlocPose.ResponseStream.Current.Pose.Position.X; + XlocData.Y = XlocPose.ResponseStream.Current.Pose.Position.Y; + XlocData.Theta = QuaternionToRPY(XlocPose.ResponseStream.Current.Pose.Orientation.X, + XlocPose.ResponseStream.Current.Pose.Orientation.Y, + XlocPose.ResponseStream.Current.Pose.Orientation.Z, + XlocPose.ResponseStream.Current.Pose.Orientation.W).yaw; + + if (TimerCounter++ >= 10) + { + if (XlocDiagnostics is null || !await XlocDiagnostics.ResponseStream.MoveNext()) + { + XlocDiagnostics = XlocClient.GetDiagnostics(new()); + return; + } + + XlocData.SlamState = XlocDiagnostics.ResponseStream.Current.SlamState; + XlocData.SlamStateDetail = XlocDiagnostics.ResponseStream.Current.SlamStateDetail; + XlocData.CurrentActiveMap = XlocDiagnostics.ResponseStream.Current.CurrentActiveMap; + XlocData.Reliability = XlocDiagnostics.ResponseStream.Current.Reliability; + XlocData.MatchingScore = XlocDiagnostics.ResponseStream.Current.MatchingScore; + } + } + catch (RpcException rpcEx) + { + XlocData.IsReady = false; + Logger.Warning($"Lỗi giao tiếp rpc với module Xloc: {rpcEx.Message}"); + await Task.Delay(1000); + } + catch (Exception ex) + { + XlocData.IsReady = false; + Logger.Warning($"Lỗi giao tiếp với module Xloc: {ex.Message}"); + await Task.Delay(1000); + } + } + + protected override async Task ExecuteAsync(CancellationToken stoppingToken) + { + if (IsSimulation) return; + + while (!stoppingToken.IsCancellationRequested) + { + XlocPose = XlocClient.GetPose(new(), cancellationToken: stoppingToken); + XlocDiagnostics = XlocClient.GetDiagnostics(new(), cancellationToken: stoppingToken); + if (await XlocPose.ResponseStream.MoveNext(stoppingToken) && await XlocPose.ResponseStream.MoveNext(stoppingToken)) break; + await Task.Delay(2000); + } + + ReaderTimer = new(ReaderInterval, XlocReader, Logger); + ReaderTimer.Start(); + + XlocData.IsReady = true; + } + + public override Task StopAsync(CancellationToken cancellationToken) + { + ReaderTimer?.Dispose(); + return base.StopAsync(cancellationToken); } } diff --git a/RobotApp/Services/Robot/RobotOrderController.cs b/RobotApp/Services/Robot/RobotOrderController.cs index 97d595b..7861839 100644 --- a/RobotApp/Services/Robot/RobotOrderController.cs +++ b/RobotApp/Services/Robot/RobotOrderController.cs @@ -154,6 +154,16 @@ public class RobotOrderController(INavigation NavigationManager, IInstantActions EdgeStates = []; } + public void PauseOrder() + { + throw new NotImplementedException(); + } + + public void ResumeOrder() + { + throw new NotImplementedException(); + } + private void HandleNavigationStart() { OrderTimer = new(CycleHandlerMilliseconds, OrderHandler, Logger); diff --git a/RobotApp/Services/RobotExtensions.cs b/RobotApp/Services/RobotExtensions.cs index c9d435f..2203a8a 100644 --- a/RobotApp/Services/RobotExtensions.cs +++ b/RobotApp/Services/RobotExtensions.cs @@ -18,12 +18,13 @@ public static class RobotExtensions services.AddInterfaceServiceSingleton(); services.AddInterfaceServiceSingleton(); services.AddInterfaceServiceSingleton(); - services.AddInterfaceServiceSingleton(); - services.AddInterfaceServiceSingleton(); + services.AddInterfaceServiceSingleton(); services.AddInterfaceServiceSingleton(); - services.AddHostedInterfaceServiceSingleton(); services.AddInterfaceServiceSingleton(); + services.AddHostedInterfaceServiceSingleton(); + services.AddHostedInterfaceServiceSingleton(); + services.AddHostedServiceSingleton(); services.AddHostedServiceSingleton(); return services; diff --git a/RobotApp/Services/State/RobotState.cs b/RobotApp/Services/State/RobotState.cs index 5640e50..d70857d 100644 --- a/RobotApp/Services/State/RobotState.cs +++ b/RobotApp/Services/State/RobotState.cs @@ -14,7 +14,6 @@ public abstract class RobotState(Enum name, IRobotState? superState = null) : public event Action? OnEnter; public event Action? OnExit; - public event Action? OnTransition; public virtual void Enter() {