278 lines
12 KiB
C#
278 lines
12 KiB
C#
using RobotNet.VDA5050;
|
|
using RobotNet.VDA5050.State;
|
|
using RobotNet.VDA5050.Type;
|
|
using RobotNet10.Common;
|
|
using RobotNet10.RobotApp.Interfaces;
|
|
using RobotNet10.RobotApp.Services.Exceptions;
|
|
using RobotNet10.RobotApp.Services.Robot.Actions;
|
|
using RobotNet10.RobotApp.Services.Robot.Helper;
|
|
using System.Collections.Concurrent;
|
|
using System.Diagnostics;
|
|
|
|
namespace RobotNet10.RobotApp.Services.Robot;
|
|
|
|
public class RobotActionController(ILogger<RobotActionController> Logger,
|
|
IRobotActionProvider RobotActionProvider,
|
|
IError ErrorManager,
|
|
INavigation NavigationManager,
|
|
IServiceScopeFactory ServiceScopeFactory) : BackgroundService, IAction
|
|
{
|
|
public ActionState[] ActionStates => [.. Actions.Values.OrderBy(a => a.SequenceNumber).Select(a => new ActionState
|
|
{
|
|
ActionId = a.Id,
|
|
ActionType = a.Type.ToJsonString(),
|
|
ActionDescription = a.Description,
|
|
ActionStatus = a.Status,
|
|
ResultDescription = a.ResultDescription,
|
|
})];
|
|
public bool HasActionRunning => !ActionQueue.IsEmpty || Actions.Values.Any(a => a.Type != ActionType.CANCEL_ORDER && !a.IsCompleted);
|
|
public bool HasActionWaitting => !ActionQueue.IsEmpty;
|
|
|
|
private readonly ConcurrentDictionary<string, RobotAction> Actions = [];
|
|
private readonly ConcurrentQueue<(ActionScope scope, RobotNet.VDA5050.InstantAction.Action action)> ActionQueue = [];
|
|
private readonly ActionConflictDetector _conflictDetector = new();
|
|
|
|
private WatchThread<RobotActionController>? HandlerTimer;
|
|
private const int HandlerInterval = 200;
|
|
private const int CompletedActionRetentionMs = 300000; // 5 minutes
|
|
private int _cleanupCounter = 0;
|
|
private const int CleanupIntervalIterations = 50; // Cleanup every 50 iterations (10 seconds)
|
|
private volatile bool _isClearing = false;
|
|
private long _sequenceCounter = 0;
|
|
|
|
public RobotAction? this[string actionId] => Actions.TryGetValue(actionId, out RobotAction? action) && action is not null ? action : null;
|
|
|
|
public void AddInstantAction(RobotNet.VDA5050.InstantAction.Action[] actions)
|
|
{
|
|
using var scope = ServiceScopeFactory.CreateAsyncScope();
|
|
var OrderManager = scope.ServiceProvider.GetRequiredService<IOrder>();
|
|
foreach (var action in actions)
|
|
{
|
|
if (Actions.TryGetValue(action.ActionId, out _)) continue;
|
|
|
|
// VDA5050: Check for conflicts with ALL running actions (ORDER + INSTANT)
|
|
var runningActions = Actions.Values.Where(a => !a.IsCompleted).ToList();
|
|
bool isOrderActive = OrderManager.NodeStates.Length > 0 || OrderManager.EdgeStates.Length > 0;
|
|
bool isDriving = NavigationManager.Driving;
|
|
|
|
var conflictResult = _conflictDetector.CheckConflict(action, runningActions, isOrderActive, isDriving);
|
|
|
|
if (conflictResult.HasConflict)
|
|
{
|
|
// Reject instant action and report error
|
|
var error = new RobotError
|
|
{
|
|
ErrorType = "instantActionConflict",
|
|
ErrorLevel = ErrorLevel.WARNING,
|
|
ErrorDescription = $"INSTANT action {action.ActionType} bị từ chối: {conflictResult.Description}",
|
|
ErrorReferences = [
|
|
new() { ReferenceKey = "actionId", ReferenceValue = action.ActionId },
|
|
new() { ReferenceKey = "conflictType", ReferenceValue = conflictResult.Type.ToString() }
|
|
]
|
|
};
|
|
|
|
if (!string.IsNullOrEmpty(conflictResult.ConflictingActionId))
|
|
{
|
|
error.ErrorReferences = [
|
|
.. error.ErrorReferences,
|
|
new() { ReferenceKey = "conflictingActionId", ReferenceValue = conflictResult.ConflictingActionId }
|
|
];
|
|
}
|
|
|
|
ErrorManager.AddError(error, TimeSpan.FromSeconds(10));
|
|
Logger.LogWarning($"INSTANT action {action.ActionId} (type: {action.ActionType}) rejected due to conflict: {conflictResult.Description}");
|
|
continue; // Skip this action
|
|
}
|
|
|
|
// No conflict - add to queue
|
|
ActionQueue.Enqueue((ActionScope.INSTANT, action));
|
|
}
|
|
}
|
|
|
|
public void AddOrderActions(RobotNet.VDA5050.InstantAction.Action[] actions, ActionScope scope = ActionScope.NODE)
|
|
{
|
|
foreach (var action in actions)
|
|
{
|
|
if (Actions.TryGetValue(action.ActionId, out _)) continue;
|
|
ActionQueue.Enqueue((scope, action));
|
|
}
|
|
}
|
|
|
|
public IEnumerable<RobotAction> GetRunningActions()
|
|
{
|
|
return Actions.Values.Where(a => !a.IsCompleted);
|
|
}
|
|
|
|
public void StartOrderAction(string actionId)
|
|
{
|
|
if (Actions.TryGetValue(actionId, out RobotAction? robotAction) && robotAction is not null)
|
|
{
|
|
robotAction.Start();
|
|
}
|
|
}
|
|
|
|
public void StopOrderAction(string actionId = "")
|
|
{
|
|
if (string.IsNullOrEmpty(actionId))
|
|
{
|
|
foreach (var action in Actions.Values)
|
|
{
|
|
if (!action.IsCompleted && action.Type != ActionType.CANCEL_ORDER) action.Cancel();
|
|
}
|
|
}
|
|
else if (Actions.TryGetValue(actionId, out RobotAction? robotAction) && robotAction is not null) robotAction.Cancel();
|
|
}
|
|
|
|
public void FinishAction(string actionId)
|
|
{
|
|
if (Actions.TryGetValue(actionId, out RobotAction? robotAction) && robotAction is not null)
|
|
{
|
|
robotAction.Finish();
|
|
}
|
|
}
|
|
|
|
public void PauseActions()
|
|
{
|
|
foreach (var action in Actions.Values)
|
|
{
|
|
action.Pause();
|
|
}
|
|
}
|
|
|
|
public void ResumeActions()
|
|
{
|
|
foreach (var action in Actions.Values)
|
|
{
|
|
action.Resume();
|
|
}
|
|
}
|
|
|
|
private void ActionHandler()
|
|
{
|
|
if (_isClearing) return;
|
|
|
|
while (!ActionQueue.IsEmpty)
|
|
{
|
|
if (!ActionQueue.TryDequeue(out var result)) continue;
|
|
if (Actions.ContainsKey(result.action.ActionId)) continue;
|
|
|
|
RobotAction? robotAction = null;
|
|
try
|
|
{
|
|
if (EnumHelper.TryParse(result.action.ActionType, out ActionType actionType))
|
|
{
|
|
robotAction = RobotActionProvider.GetRobotAction(actionType);
|
|
if (robotAction is not null)
|
|
{
|
|
robotAction.Initialize(result.scope, result.action);
|
|
robotAction.SequenceNumber = Interlocked.Increment(ref _sequenceCounter);
|
|
Actions.TryAdd(result.action.ActionId, robotAction);
|
|
if (result.scope == ActionScope.INSTANT) robotAction.Start();
|
|
}
|
|
}
|
|
else
|
|
{
|
|
var error = new RobotError
|
|
{
|
|
ErrorType = "actionTypeInvalid",
|
|
ErrorLevel = RobotNet.VDA5050.Type.ErrorLevel.WARNING,
|
|
ErrorDescription = $"ActionType không hợp lệ: {result.action.ActionType}",
|
|
ErrorReferences = [new() { ReferenceKey = "actionId", ReferenceValue = result.action.ActionId }]
|
|
};
|
|
ErrorManager.AddError(error, TimeSpan.FromSeconds(10));
|
|
Logger.LogWarning("ActionType không hợp lệ: {ActionType} cho action {ActionId}", result.action.ActionType, result.action.ActionId);
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
var errorMsg = ex is RobotException rex && rex.Error is not null ? rex.Error.ErrorDescription : ex.Message;
|
|
|
|
// Nếu robotAction đã tạo, mark FAILED và add vào Actions để FM nhận được trạng thái
|
|
if (robotAction is not null)
|
|
{
|
|
robotAction.Cancel();
|
|
robotAction.ResultDescription = $"Khởi tạo action thất bại: {errorMsg}";
|
|
robotAction.SequenceNumber = Interlocked.Increment(ref _sequenceCounter);
|
|
Actions.TryAdd(result.action.ActionId, robotAction);
|
|
}
|
|
|
|
var error = new RobotError
|
|
{
|
|
ErrorType = "actionInitializationFailed",
|
|
ErrorLevel = RobotNet.VDA5050.Type.ErrorLevel.WARNING,
|
|
ErrorDescription = $"Action {result.action.ActionId} ({result.action.ActionType}) khởi tạo thất bại: {errorMsg}",
|
|
ErrorReferences = [new() { ReferenceKey = "actionId", ReferenceValue = result.action.ActionId }]
|
|
};
|
|
ErrorManager.AddError(error, TimeSpan.FromSeconds(10));
|
|
Logger.LogWarning("Action {ActionId} (type: {ActionType}) initialization failed: {Error}", result.action.ActionId, result.action.ActionType, errorMsg);
|
|
}
|
|
}
|
|
|
|
// Cleanup completed actions periodically to prevent memory leak
|
|
_cleanupCounter++;
|
|
if (_cleanupCounter >= CleanupIntervalIterations)
|
|
{
|
|
_cleanupCounter = 0;
|
|
CleanupCompletedActions();
|
|
}
|
|
}
|
|
|
|
private void CleanupCompletedActions()
|
|
{
|
|
try
|
|
{
|
|
long currentTime = Stopwatch.GetTimestamp() * 1000 / Stopwatch.Frequency;
|
|
var actionsToRemove = Actions.Where(kvp =>
|
|
kvp.Value.IsCompleted &&
|
|
kvp.Value.CompletionTime > 0 &&
|
|
(currentTime - kvp.Value.CompletionTime) > CompletedActionRetentionMs
|
|
).Select(kvp => kvp.Key).ToList();
|
|
|
|
foreach (var actionId in actionsToRemove)
|
|
{
|
|
if (Actions.TryGetValue(actionId, out var action))
|
|
{
|
|
_ = action.DisposeAsync(); // Fire and forget disposal
|
|
Actions.TryRemove(actionId, out _);
|
|
Logger.LogDebug($"Cleaned up completed action: {actionId} (Type: {action.Type})");
|
|
}
|
|
}
|
|
|
|
if (actionsToRemove.Count > 0)
|
|
{
|
|
Logger.LogInformation($"Cleaned up {actionsToRemove.Count} completed actions");
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Logger.LogWarning($"Error during action cleanup: {ex.Message}");
|
|
}
|
|
}
|
|
|
|
public async Task ClearActions()
|
|
{
|
|
_isClearing = true;
|
|
ActionQueue.Clear();
|
|
var disposeTasks = Actions.Values.Select(action => action.DisposeAsync().AsTask()).ToList();
|
|
await Task.WhenAll(disposeTasks).ConfigureAwait(false);
|
|
Actions.Clear();
|
|
ActionQueue.Clear(); // Clear lần nữa phòng trường hợp có action mới enqueue trong lúc dispose
|
|
Interlocked.Exchange(ref _sequenceCounter, 0);
|
|
_isClearing = false;
|
|
}
|
|
|
|
protected override async Task ExecuteAsync(CancellationToken stoppingToken)
|
|
{
|
|
await Task.Yield();
|
|
HandlerTimer = new(HandlerInterval, ActionHandler, Logger);
|
|
HandlerTimer.Start();
|
|
}
|
|
|
|
public override Task StopAsync(CancellationToken cancellationToken)
|
|
{
|
|
HandlerTimer?.Dispose();
|
|
HandlerTimer = null;
|
|
return base.StopAsync(cancellationToken);
|
|
}
|
|
}
|