95 lines
3.8 KiB
C#
95 lines
3.8 KiB
C#
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;
|
|
}
|
|
}
|