361 lines
12 KiB
C#
361 lines
12 KiB
C#
using Appccelerate.StateMachine;
|
|
using Appccelerate.StateMachine.Machine;
|
|
using RobotNet.VDA5050;
|
|
using RobotNet.VDA5050.Type;
|
|
using RobotNet10.Common;
|
|
using RobotNet10.NavigationTune.Shared.Models;
|
|
using RobotNet10.RobotApp.Services.Exceptions;
|
|
using System.Diagnostics;
|
|
using System.Reflection;
|
|
|
|
namespace RobotNet10.RobotApp.Services.Robot.Actions;
|
|
|
|
public abstract class RobotAction : IAsyncDisposable
|
|
{
|
|
public ActionType Type { get; }
|
|
public string Id { get; private set; } = "";
|
|
public string? Description { get; private set; }
|
|
public BlockingType BlockingType { get; private set; }
|
|
public RobotNet.VDA5050.InstantAction.ActionParameter[] Parameters { get; protected set; } = [];
|
|
public ActionStatus Status => CurrentStatus;
|
|
public string ResultDescription { get; set; } = "";
|
|
public bool IsCompleted => CurrentStatus == ActionStatus.FINISHED || CurrentStatus == ActionStatus.FAILED;
|
|
public long CompletionTime { get; private set; } = 0;
|
|
public ActionScope ActionScope { get; set; }
|
|
public RobotActionAttribute ActionAttribute { get; }
|
|
public long SequenceNumber { get; internal set; }
|
|
|
|
private WatchThreadAsync<RobotAction>? ActionTimer;
|
|
protected const int ActionInterval = 100;
|
|
|
|
protected IServiceProvider ServiceProvider;
|
|
protected RobotNet.VDA5050.InstantAction.Action? Action;
|
|
protected ILogger<RobotAction>? Logger;
|
|
protected bool IsPaused = false;
|
|
protected ActionStatus HistoryStatus;
|
|
private bool _justResumed = false;
|
|
|
|
private bool _isDisposed = false;
|
|
private bool IsCancelAction = false;
|
|
private PassiveStateMachine<ActionStatus, ActionEvent>? _stateMachine;
|
|
private ActionStatus CurrentStatus;
|
|
private long StartTime;
|
|
private int Timeout;
|
|
|
|
private readonly Lock _lock = new();
|
|
|
|
public RobotAction(IServiceProvider serviceProvider)
|
|
{
|
|
var derivedType = GetType();
|
|
|
|
ActionAttribute = derivedType.GetCustomAttribute<RobotActionAttribute>()
|
|
?? throw new InvalidOperationException(
|
|
$"Class {derivedType.Name} must have RobotActionAttribute");
|
|
|
|
Type = ActionAttribute.ActionType;
|
|
ServiceProvider = serviceProvider;
|
|
|
|
Logger = ServiceProvider.GetRequiredService<ILogger<RobotAction>>();
|
|
InitializeStatus();
|
|
}
|
|
|
|
public void Initialize(ActionScope actionScope, RobotNet.VDA5050.InstantAction.Action action)
|
|
{
|
|
ActionScope = actionScope;
|
|
Action = action;
|
|
BlockingType = action.BlockingType;
|
|
Id = action.ActionId;
|
|
Description = action.ActionDescription;
|
|
Initialize();
|
|
}
|
|
|
|
public void Start()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (Status != ActionStatus.WAITING) return;
|
|
ActionTimer = new(ActionInterval, ActionHandler, Logger);
|
|
SetStatus(ActionEvent.INITIALIZING);
|
|
ActionTimer.Start();
|
|
StartTime = GetCurrentTimeMs();
|
|
}
|
|
}
|
|
|
|
public void Pause()
|
|
{
|
|
if (_isDisposed) return;
|
|
lock (_lock)
|
|
{
|
|
if (IsCompleted) return;
|
|
HistoryStatus = Status;
|
|
SetStatus(ActionEvent.PAUSED);
|
|
IsPaused = true;
|
|
}
|
|
}
|
|
|
|
public void Resume()
|
|
{
|
|
if (_isDisposed) return;
|
|
lock (_lock)
|
|
{
|
|
if (Status == ActionStatus.PAUSED)
|
|
{
|
|
IsPaused = false;
|
|
_justResumed = true;
|
|
// Restore về trạng thái trước khi pause
|
|
ActionEvent resumeEvent = HistoryStatus switch
|
|
{
|
|
ActionStatus.RUNNING => ActionEvent.RUNNING,
|
|
ActionStatus.INITIALIZING => ActionEvent.INITIALIZING,
|
|
_ => ActionEvent.WAITING
|
|
};
|
|
SetStatus(resumeEvent);
|
|
}
|
|
}
|
|
}
|
|
|
|
public void Cancel()
|
|
{
|
|
if (_isDisposed) return;
|
|
lock (_lock)
|
|
{
|
|
if (!IsCompleted) IsCancelAction = true;
|
|
if (Status == ActionStatus.WAITING) _ = StopAction();
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// VDA5050: Gracefully finish an action (e.g., when robot leaves an edge).
|
|
/// Sets status to FINISHED instead of FAILED (Cancel).
|
|
/// </summary>
|
|
public void Finish()
|
|
{
|
|
if (_isDisposed) return;
|
|
lock (_lock)
|
|
{
|
|
if (IsCompleted) return;
|
|
// State machine allows FINISHED from RUNNING and INITIALIZING
|
|
if (CurrentStatus == ActionStatus.RUNNING || CurrentStatus == ActionStatus.INITIALIZING)
|
|
{
|
|
SetStatus(ActionEvent.FINISHED);
|
|
if (string.IsNullOrEmpty(ResultDescription))
|
|
ResultDescription = "Action completed (edge transition).";
|
|
}
|
|
else
|
|
{
|
|
// WAITING or PAUSED: cancel as fallback
|
|
IsCancelAction = true;
|
|
}
|
|
}
|
|
}
|
|
|
|
protected virtual Task StartAction()
|
|
{
|
|
return Task.CompletedTask;
|
|
}
|
|
|
|
protected virtual Task StopAction()
|
|
{
|
|
SetStatus(ActionEvent.FAILED);
|
|
ResultDescription = "Action bị hủy bỏ.";
|
|
return Task.CompletedTask;
|
|
}
|
|
|
|
protected virtual Task CompleteAction()
|
|
{
|
|
return Task.CompletedTask;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Giải phóng tài nguyên được sử dụng trong action.
|
|
/// Luôn được gọi trong DisposeAsync, đảm bảo cleanup cả khi FINISHED lẫn FAILED/Cancel.
|
|
/// </summary>
|
|
protected virtual Task CleanupAction()
|
|
{
|
|
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 void Initialize()
|
|
{
|
|
if (Action is null) throw new ActionException("Khởi tạo Action không tồn tại");
|
|
if (EnumHelper.TryParse(Action.ActionType, out ActionType type))
|
|
{
|
|
if (type != Type) throw new ActionException($"ActionType {Action.ActionType} không khớp với action hiện tại {Type}.");
|
|
}
|
|
else throw new ActionException($"ActionType {Action.ActionType} không hợp lệ.");
|
|
|
|
if (!ActionAttribute.BlockingTypes.Any(bt => bt == BlockingType)) throw new ActionException($"BlockingType {BlockingType} không được hỗ trợ cho action {Type}.");
|
|
if (!ActionAttribute.ActionScopes.Any(sp => sp == ActionScope)) throw new ActionException($"ActionScope {ActionScope} không được hỗ trợ cho action {Type}.");
|
|
|
|
if (Action.ActionParameters != null && Action.ActionParameters.Length > 0)
|
|
{
|
|
var para = Action.ActionParameters.FirstOrDefault(p => p.Key == "timeout");
|
|
if (para is not null && int.TryParse(para.Value, out int miliseconds) && miliseconds > 100) Timeout = miliseconds;
|
|
else Timeout = -1;
|
|
}
|
|
}
|
|
|
|
private async Task ActionHandler()
|
|
{
|
|
try
|
|
{
|
|
if (Timeout > 0)
|
|
{
|
|
long now = GetCurrentTimeMs();
|
|
if (now >= (StartTime + Timeout)) throw new TimeoutException($"Action [{Type} - {Id}] timeout. Timeout: {Timeout} ms ");
|
|
}
|
|
|
|
ActionStatus status;
|
|
bool isCancel = false;
|
|
bool justResumed = false;
|
|
lock (_lock)
|
|
{
|
|
status = CurrentStatus;
|
|
isCancel = IsCancelAction;
|
|
justResumed = _justResumed;
|
|
if (_justResumed) _justResumed = false;
|
|
}
|
|
|
|
if (isCancel)
|
|
{
|
|
await StopAction();
|
|
}
|
|
else
|
|
{
|
|
if (status == ActionStatus.INITIALIZING)
|
|
{
|
|
Logger?.LogInformation($"Executing action {Type}");
|
|
SetStatus(ActionEvent.RUNNING);
|
|
await StartAction();
|
|
}
|
|
else if (status == ActionStatus.RUNNING)
|
|
{
|
|
if (justResumed)
|
|
{
|
|
await ResumeAction();
|
|
}
|
|
await ExecuteAction();
|
|
}
|
|
else if (status == ActionStatus.PAUSED)
|
|
{
|
|
await PauseAction();
|
|
}
|
|
}
|
|
|
|
if (IsCompleted)
|
|
{
|
|
await CompleteAction();
|
|
await DisposeAsync();
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Logger?.LogError($"Action [{Type} - {Id}] execution error: {ex.Message}");
|
|
lock (_lock)
|
|
{
|
|
SetStatus(ActionEvent.FAILED);
|
|
}
|
|
ResultDescription = $"Thực hiện action [{Type} - {Id}] xảy ra lỗi: {ex.Message}";
|
|
await DisposeAsync();
|
|
}
|
|
}
|
|
|
|
private void InitializeStatus()
|
|
{
|
|
var builder = new StateMachineDefinitionBuilder<ActionStatus, ActionEvent>();
|
|
|
|
builder.In(ActionStatus.WAITING)
|
|
.ExecuteOnEntry(() => { CurrentStatus = ActionStatus.WAITING; })
|
|
.On(ActionEvent.INITIALIZING).Goto(ActionStatus.INITIALIZING)
|
|
.On(ActionEvent.PAUSED).Goto(ActionStatus.PAUSED)
|
|
.On(ActionEvent.FAILED).Goto(ActionStatus.FAILED);
|
|
|
|
|
|
builder.In(ActionStatus.INITIALIZING)
|
|
.ExecuteOnEntry(() => { CurrentStatus = ActionStatus.INITIALIZING; })
|
|
.On(ActionEvent.RUNNING).Goto(ActionStatus.RUNNING)
|
|
.On(ActionEvent.PAUSED).Goto(ActionStatus.PAUSED)
|
|
.On(ActionEvent.FINISHED).Goto(ActionStatus.FINISHED)
|
|
.On(ActionEvent.FAILED).Goto(ActionStatus.FAILED);
|
|
|
|
builder.In(ActionStatus.RUNNING)
|
|
.ExecuteOnEntry(() => { CurrentStatus = ActionStatus.RUNNING; })
|
|
.On(ActionEvent.PAUSED).Goto(ActionStatus.PAUSED)
|
|
.On(ActionEvent.FINISHED).Goto(ActionStatus.FINISHED)
|
|
.On(ActionEvent.FAILED).Goto(ActionStatus.FAILED);
|
|
|
|
builder.In(ActionStatus.PAUSED)
|
|
.ExecuteOnEntry(() => { CurrentStatus = ActionStatus.PAUSED; })
|
|
.On(ActionEvent.WAITING).Goto(ActionStatus.WAITING)
|
|
.On(ActionEvent.INITIALIZING).Goto(ActionStatus.INITIALIZING)
|
|
.On(ActionEvent.RUNNING).Goto(ActionStatus.RUNNING)
|
|
.On(ActionEvent.FAILED).Goto(ActionStatus.FAILED);
|
|
|
|
builder.In(ActionStatus.FINISHED)
|
|
.ExecuteOnEntry(() =>
|
|
{
|
|
CurrentStatus = ActionStatus.FINISHED;
|
|
CompletionTime = GetCurrentTimeMs();
|
|
});
|
|
|
|
builder.In(ActionStatus.FAILED)
|
|
.ExecuteOnEntry(() =>
|
|
{
|
|
CurrentStatus = ActionStatus.FAILED;
|
|
CompletionTime = GetCurrentTimeMs();
|
|
});
|
|
|
|
_stateMachine = builder
|
|
.WithInitialState(ActionStatus.WAITING)
|
|
.Build()
|
|
.CreatePassiveStateMachine();
|
|
|
|
_stateMachine.Start();
|
|
}
|
|
|
|
private static long GetCurrentTimeMs()
|
|
{
|
|
return Stopwatch.GetTimestamp() * 1000 / Stopwatch.Frequency;
|
|
}
|
|
|
|
protected void SetStatus(ActionEvent eventStatus)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
_stateMachine?.Fire(eventStatus);
|
|
}
|
|
}
|
|
|
|
public async ValueTask DisposeAsync()
|
|
{
|
|
bool shouldStop;
|
|
lock (_lock)
|
|
{
|
|
if (_isDisposed) return;
|
|
_isDisposed = true;
|
|
shouldStop = !IsCompleted;
|
|
}
|
|
|
|
if (shouldStop) await StopAction();
|
|
await CleanupAction();
|
|
ActionTimer?.Dispose();
|
|
ActionTimer = null;
|
|
GC.SuppressFinalize(this);
|
|
}
|
|
}
|