331 lines
12 KiB
C#
331 lines
12 KiB
C#
using Microsoft.AspNetCore.SignalR.Client;
|
|
using RobotNet.Clients;
|
|
using RobotNet.RobotShares.Models;
|
|
using RobotNet.Script;
|
|
using RobotNet.Script.Expressions;
|
|
using RobotNet.ScriptManager.Helpers;
|
|
using RobotNet.Shares;
|
|
|
|
namespace RobotNet.ScriptManager.Clients;
|
|
|
|
public class RobotManagerHubClient(string RobotId, string hubUrl, Func<Task<string?>> accessTokenProvider) : HubClient(new Uri(hubUrl), accessTokenProvider), IRobot
|
|
{
|
|
private async Task<RobotStateModel> GetRobotStateModel()
|
|
{
|
|
var result = await Connection.InvokeAsync<MessageResult<RobotStateModel>>(nameof(GetState), RobotId);
|
|
if (!result.IsSuccess)
|
|
{
|
|
throw new InvalidOperationException($"Failed to get robot state: {result.Message}");
|
|
}
|
|
else
|
|
{
|
|
if (result.Data is null)
|
|
{
|
|
throw new InvalidOperationException("Robot state data is null.");
|
|
}
|
|
return result.Data;
|
|
}
|
|
}
|
|
|
|
private static string RobotErrorToString(IEnumerable<RobotNet.RobotShares.VDA5050.State.Error> errors)
|
|
{
|
|
var sb = new System.Text.StringBuilder();
|
|
foreach (var error in errors)
|
|
{
|
|
if (sb.Length > 0)
|
|
{
|
|
sb.Append(", ");
|
|
}
|
|
sb.Append($"{error.ErrorLevel}: {error.ErrorType} {error.ErrorDescription} - {error.ErrorHint}");
|
|
foreach (var reference in error.ErrorReferences ?? [])
|
|
{
|
|
sb.Append($" [{reference.ReferenceKey}: {reference.ReferenceValue}]");
|
|
}
|
|
}
|
|
return sb.ToString();
|
|
}
|
|
|
|
public async Task<RobotState> GetState() => VDA5050ScriptHelper.ConvertToRobotState(await GetRobotStateModel());
|
|
|
|
public async Task<RobotLoad[]> GetLoads()
|
|
{
|
|
var stateModel = await GetRobotStateModel();
|
|
if (stateModel.Loads is null || stateModel.Loads.Length == 0)
|
|
{
|
|
return [];
|
|
}
|
|
return [.. stateModel.Loads.Select(load => new RobotLoad(load.LoadId, load.LoadType, load.LoadPosition, load.Weight))];
|
|
}
|
|
|
|
public async Task<RobotResult> Execute(RobotAction action, CancellationToken cancellationToken)
|
|
{
|
|
var state = await GetState();
|
|
if (!state.IsReady)
|
|
{
|
|
throw new InvalidOperationException($"Robot '{RobotId}' không sẵn sàng để thực hiện action [{action.ActionType}]!");
|
|
}
|
|
|
|
var model = VDA5050ScriptHelper.ConvertToRobotInstantActionModel(RobotId, action);
|
|
var result = await Connection.InvokeAsync<MessageResult<string>>("InstantAction", model, cancellationToken);
|
|
|
|
if (!result.IsSuccess)
|
|
{
|
|
throw new InvalidOperationException($"Failed to perform instant action: {result.Message}");
|
|
}
|
|
|
|
if (string.IsNullOrEmpty(result.Data))
|
|
{
|
|
throw new InvalidOperationException("Instant action ID is empty.");
|
|
}
|
|
|
|
bool isRunning = true;
|
|
bool isSuccess = false;
|
|
string message = "";
|
|
while (isRunning)
|
|
{
|
|
if (cancellationToken.IsCancellationRequested)
|
|
{
|
|
isRunning = false;
|
|
message = $"Action [{action.ActionType} is canceled";
|
|
break;
|
|
}
|
|
|
|
var stateModel = await GetRobotStateModel();
|
|
var stateAction = stateModel.ActionStates.FirstOrDefault(a => a.ActionId == result.Data);
|
|
if (stateAction is null)
|
|
{
|
|
isRunning = false;
|
|
message = $"Action [{action.ActionType}] not found in robot state.";
|
|
}
|
|
else if (stateAction.IsError)
|
|
{
|
|
isRunning = false;
|
|
message = $"Action [{action.ActionType}] failed with result: {stateAction.Action?.ResultDescription}, Error: {RobotErrorToString(stateModel.Errors)}";
|
|
}
|
|
else if (stateAction.IsProcessing)
|
|
{
|
|
if (cancellationToken.IsCancellationRequested)
|
|
{
|
|
isRunning = false;
|
|
message = $"Action [{action.ActionType}] wait cancel";
|
|
}
|
|
else
|
|
{
|
|
try
|
|
{
|
|
await Task.Delay(50, cancellationToken);
|
|
}
|
|
catch (TaskCanceledException)
|
|
{
|
|
isRunning = false;
|
|
message = $"Action [{action.ActionType}] wait cancel";
|
|
}
|
|
}
|
|
}
|
|
else if (stateAction.IsCompleted)
|
|
{
|
|
isRunning = false;
|
|
isSuccess = true;
|
|
}
|
|
else
|
|
{
|
|
isRunning = false;
|
|
message = $"Action [{action.ActionType}] is in an unexpected state";
|
|
}
|
|
}
|
|
return new(isSuccess, message);
|
|
}
|
|
|
|
public async Task AbortMovement()
|
|
{
|
|
var result = await Connection.InvokeAsync<MessageResult>("CancelOrder", RobotId);
|
|
if (!result.IsSuccess)
|
|
{
|
|
throw new InvalidOperationException($"Failed to cancel robot operation: {result.Message}");
|
|
}
|
|
}
|
|
|
|
private async Task<RobotResult> WaitOrder(string premessage, CancellationToken cancellationToken)
|
|
{
|
|
bool isRunning = true;
|
|
bool isSuccess = false;
|
|
string message = "";
|
|
while (isRunning)
|
|
{
|
|
if (cancellationToken.IsCancellationRequested)
|
|
{
|
|
message = $"{premessage} is canceled";
|
|
break;
|
|
}
|
|
var stateModel = await GetRobotStateModel();
|
|
if (stateModel.OrderState.IsError)
|
|
{
|
|
isRunning = false;
|
|
message = $"{premessage} failed with error: {string.Join("\n\t", stateModel.OrderState.Errors)} \n Robot Error: {RobotErrorToString(stateModel.Errors)}";
|
|
}
|
|
else if (stateModel.OrderState.IsProcessing)
|
|
{
|
|
if (cancellationToken.IsCancellationRequested)
|
|
{
|
|
isRunning = false;
|
|
message = $"{premessage} is canceled";
|
|
}
|
|
else
|
|
{
|
|
try
|
|
{
|
|
await Task.Delay(50, cancellationToken);
|
|
}
|
|
catch (TaskCanceledException)
|
|
{
|
|
isRunning = false;
|
|
message = $"{premessage} is canceled";
|
|
}
|
|
}
|
|
}
|
|
else if (stateModel.OrderState.IsCompleted)
|
|
{
|
|
isRunning = false;
|
|
isSuccess = true;
|
|
}
|
|
else
|
|
{
|
|
isRunning = false;
|
|
message = $"{premessage} is in an unexpected state";
|
|
}
|
|
}
|
|
return new(isSuccess, message);
|
|
}
|
|
|
|
private async Task<RobotResult> MoveToNode(string nodeName, IDictionary<string, IEnumerable<RobotAction>> actions, double? lastAngle, CancellationToken cancellationToken)
|
|
{
|
|
var state = await GetState();
|
|
if (!state.IsReady)
|
|
{
|
|
throw new InvalidOperationException($"Robot '{RobotId}' không sẵn sàng để thực hiện di chuyển đến [{nodeName}]!");
|
|
}
|
|
|
|
var model = VDA5050ScriptHelper.ConvertToRobotMoveToNodeModel(RobotId, nodeName, actions, lastAngle);
|
|
|
|
var result = await Connection.InvokeAsync<MessageResult>("MoveToNode", model, cancellationToken);
|
|
|
|
if (!result.IsSuccess)
|
|
{
|
|
throw new InvalidOperationException($"Failed to move to node: {result.Message}");
|
|
}
|
|
|
|
return await WaitOrder($"Robot move to node [{nodeName}]", cancellationToken);
|
|
}
|
|
|
|
public Task<RobotResult> Move(string nodeName, IDictionary<string, IEnumerable<RobotAction>> actions, double lastAngle, CancellationToken cancellationToken)
|
|
=> MoveToNode(nodeName, actions, lastAngle, cancellationToken);
|
|
|
|
public Task<RobotResult> Move(string nodeName, IDictionary<string, IEnumerable<RobotAction>> actions, CancellationToken cancellationToken)
|
|
=> MoveToNode(nodeName, actions, null, cancellationToken);
|
|
|
|
public Task<RobotResult> Move(string nodeName, IEnumerable<RobotAction> lastActions, double lastAngle, CancellationToken cancellationToken)
|
|
=> MoveToNode(nodeName, new Dictionary<string, IEnumerable<RobotAction>> { { nodeName, lastActions } }, lastAngle, cancellationToken);
|
|
|
|
public Task<RobotResult> Move(string nodeName, IEnumerable<RobotAction> lastActions, CancellationToken cancellationToken)
|
|
=> MoveToNode(nodeName, new Dictionary<string, IEnumerable<RobotAction>> { { nodeName, lastActions } }, null, cancellationToken);
|
|
|
|
public Task<RobotResult> Move(string nodeName, double lastAngle, CancellationToken cancellationToken)
|
|
=> MoveToNode(nodeName, new Dictionary<string, IEnumerable<RobotAction>>(), lastAngle, cancellationToken);
|
|
|
|
public Task<RobotResult> Move(string nodeName, CancellationToken cancellationToken)
|
|
=> MoveToNode(nodeName, new Dictionary<string, IEnumerable<RobotAction>>(), null, cancellationToken);
|
|
|
|
public async Task<RobotResult> SimMoveStraight(double x, double y, CancellationToken cancellationToken)
|
|
{
|
|
var state = await GetState();
|
|
if (!state.IsReady)
|
|
{
|
|
throw new InvalidOperationException($"Robot '{RobotId}' không sẵn sàng để thực hiện di chuyển đến [{x}; {y}]!");
|
|
}
|
|
|
|
var result = await Connection.InvokeAsync<MessageResult>("MoveStraight", new RobotMoveStraightModel()
|
|
{
|
|
RobotId = RobotId,
|
|
X = x,
|
|
Y = y
|
|
}, cancellationToken);
|
|
|
|
|
|
if (!result.IsSuccess)
|
|
{
|
|
throw new InvalidOperationException($"Failed to move straight to [{x}; {y}]: {result.Message}");
|
|
}
|
|
|
|
return await WaitOrder($"Robot move straight to [{x}; {y}]", cancellationToken);
|
|
}
|
|
|
|
public async Task<RobotResult> SimRotate(double angle, CancellationToken cancellationToken)
|
|
{
|
|
var state = await GetState();
|
|
if (!state.IsReady)
|
|
{
|
|
throw new InvalidOperationException($"Robot '{RobotId}' không sẵn sàng để thực hiện rotate to [{angle}]!");
|
|
}
|
|
|
|
var result = await Connection.InvokeAsync<MessageResult>("Rotate", new RobotRotateModel()
|
|
{
|
|
RobotId = RobotId,
|
|
Angle = angle
|
|
}, cancellationToken);
|
|
|
|
|
|
if (!result.IsSuccess)
|
|
{
|
|
throw new InvalidOperationException($"Failed to rotate to [{angle}]: {result.Message}");
|
|
}
|
|
|
|
return await WaitOrder($"Robot rotate to [{angle}]", cancellationToken);
|
|
}
|
|
|
|
public async Task<bool> WaitForReady(CancellationToken cancellationToken)
|
|
{
|
|
var state = await GetState();
|
|
while(!state.IsReady && !cancellationToken.IsCancellationRequested)
|
|
{
|
|
try
|
|
{
|
|
await Task.Delay(50, cancellationToken);
|
|
}
|
|
catch (TaskCanceledException)
|
|
{
|
|
return false; // Operation was canceled
|
|
}
|
|
state = await GetState();
|
|
}
|
|
if (cancellationToken.IsCancellationRequested)
|
|
{
|
|
return false; // Operation was canceled
|
|
}
|
|
return state.IsReady;
|
|
}
|
|
|
|
public async Task<bool> RequestACSIn(string id)
|
|
{
|
|
var result = await Connection.InvokeAsync<MessageResult>(nameof(RequestACSIn), RobotId, id);
|
|
return result.IsSuccess;
|
|
}
|
|
|
|
public async Task<bool> RequestACSOut(string id)
|
|
{
|
|
var result = await Connection.InvokeAsync<MessageResult>(nameof(RequestACSOut), RobotId, id);
|
|
return result.IsSuccess;
|
|
}
|
|
|
|
public async Task<bool> CancelOrder()
|
|
{
|
|
var result = await Connection.InvokeAsync<MessageResult>(nameof(CancelOrder), RobotId);
|
|
return result.IsSuccess;
|
|
}
|
|
|
|
public async Task<bool> CancelAction()
|
|
{
|
|
var result = await Connection.InvokeAsync<MessageResult>(nameof(CancelOrder), RobotId);
|
|
return result.IsSuccess;
|
|
}
|
|
}
|