Initial commit

2026-07-03 16:37:12 +07:00
commit 63b8c1ea8b
1931 changed files with 640587 additions and 0 deletions
--- a/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/RobotInitPositionAction.cs
+++ b/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Services/Robot/Actions/RobotInitPositionAction.cs
@@ -0,0 +1,94 @@
+using RobotNet.VDA5050.Factsheet;
+using RobotNet.VDA5050.Type;
+using RobotNet10.RobotApp.Interfaces;
+using RobotNet10.RobotApp.Services.Exceptions;
+using System.Collections.ObjectModel;
+
+namespace RobotNet10.RobotApp.Services.Robot.Actions;
+
+[RobotAction(ActionType.INIT_POSITION,
+            [ActionScope.INSTANT, ActionScope.NODE],
+            [BlockingType.HARD],
+            "Khởi tạo lại vị trí robot.",
+            "Robot đã khởi tạo lại vị trí.")]
+public class RobotInitPositionAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
+{
+    private static readonly ReadOnlyCollection<ActionParameter> ActionParameters = new List<ActionParameter>
+    {
+        new()
+        {
+            Key = "x",
+            Description = "Tọa độ X của vị trí khởi tạo.",
+            ValueDataType = ValueDataType.FLOAT,
+            IsOptional = false,
+        },
+        new()
+        {
+            Key = "y",
+            Description = "Tọa độ Y của vị trí khởi tạo.",
+            ValueDataType = ValueDataType.FLOAT,
+            IsOptional = false,
+        },
+        new()
+        {
+            Key = "theta",
+            Description = "Góc quay (theta) của vị trí khởi tạo. (rad)",
+            ValueDataType = ValueDataType.FLOAT,
+            IsOptional = false,
+        }
+    }.AsReadOnly();
+
+    double X = 0;
+    double Y = 0;
+    double Theta = 0;
+    ILocalization? Localization;
+
+    protected override Task StartAction()
+    {
+        Localization = ServiceProvider.GetRequiredService<ILocalization>();
+        var initPose = Localization.SetInitializePosition(X, Y, Theta);
+        if (!initPose.IsSuccess)
+        {
+            SetStatus(ActionEvent.FAILED);
+            ResultDescription = initPose.Message;
+        }
+        else
+        {
+            SetStatus(ActionEvent.FINISHED);
+            ResultDescription = string.IsNullOrEmpty(ActionAttribute.ResultDescription) ? ResultDescription : ActionAttribute.ResultDescription;
+        }
+        return base.StartAction();
+    }
+
+    protected override void Initialize()
+    {
+        base.Initialize();
+
+        if (ActionParameters.Count > 0)
+        {
+            foreach (var parameterStore in ActionParameters)
+            {
+                if (!parameterStore.IsOptional)
+                {
+                    if (Action is null || Action.ActionParameters is null || !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 is null || Action.ActionParameters is null ? [] : Action.ActionParameters;
+        }
+
+        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(yPara.Value, out double yData);
+        var thetaParse = double.TryParse(thetaPara.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;
+    }
+}