Initial commit
This commit is contained in:
@@ -0,0 +1,360 @@
|
||||
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);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user