DangNV/RobotApp
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update

Browse Source
This commit is contained in:
Đăng Nguyễn 2025-10-24 17:09:00 +07:00
parent a01f140f2e
commit 6eeed8c7b4
19 changed files with 288 additions and 96 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
RobotApp/Interfaces/IInstantActions.cs
View File

@ -1,4 +1,5 @@
using RobotApp.VDA5050.State;
using RobotApp.Services.Robot.Actions;
using RobotApp.VDA5050.State;
using Action = RobotApp.VDA5050.InstantAction.Action;
namespace RobotApp.Interfaces;
@ -7,9 +8,11 @@ public interface IInstantActions
{
ActionState[] ActionStates { get; }
bool HasActionRunning { get; }
RobotAction? this[string actionId] { get; }
void AddOrderActions(Action[] actions);
void AddInstantAction(Action[] action);
void StartOrderAction(string actionId, bool wait);
void StartOrderAction(string actionId, bool wait = false);
void StopOrderAction(string actionId = "");
void ClearInstantActions();
void PauseActions();
void ResumeActions();

2
RobotApp/Interfaces/INavigation.cs
View File

@ -26,7 +26,6 @@ public enum NavigationProccess
public interface INavigation
{
event Action? OnNavigationStateChanged;
event Action? OnNavigationFinished;
bool IsReady { get; }
bool Driving { get; }
@ -43,4 +42,5 @@ public interface INavigation
void RefreshOrder(Node[] nodes, Edge[] edges);
void Refresh();
void CancelMovement();
void SetSpeed(double speed);
}

60
RobotApp/Services/Robot/Actions/RobotAction.cs
View File

@ -1,4 +1,5 @@
using Grpc.Core;
using MudBlazor;
using RobotApp.Services.Exceptions;
using RobotApp.VDA5050.Factsheet;
using RobotApp.VDA5050.InstantAction;
@ -7,7 +8,7 @@ using RobotApp.VDA5050.Type;
namespace RobotApp.Services.Robot.Actions;
public abstract class RobotAction(IServiceProvider serviceProvider) : IAsyncDisposable
public abstract class RobotAction(IServiceProvider serviceProvider) : IDisposable
{
public ActionType Type { get; private set; }
public string Id { get; private set; } = "";
@ -30,6 +31,8 @@ public abstract class RobotAction(IServiceProvider serviceProvider) : IAsyncDisp
protected bool IsPaused = false;
protected ActionStatus HistoryStatus;
private bool IsCancelAction = false;
public bool Initialize(ActionScopes actionScope, VDA5050.InstantAction.Action action)
{
Status = ActionStatus.WAITING;
@ -65,7 +68,7 @@ public abstract class RobotAction(IServiceProvider serviceProvider) : IAsyncDisp
public void Cancel()
{
Dispose();
if(!IsCompleted) IsCancelAction = true;
}
public async Task WaitAsync(CancellationToken cancellationToken)
@ -89,6 +92,13 @@ public abstract class RobotAction(IServiceProvider serviceProvider) : IAsyncDisp
return Task.CompletedTask;
}
protected virtual Task StopAction()
{
Status = ActionStatus.FAILED;
ResultDescription = "Action bị hủy bỏ.";
return Task.CompletedTask;
}
protected virtual Task ExecuteAction()
{
return Task.CompletedTask;
@ -139,28 +149,35 @@ public abstract class RobotAction(IServiceProvider serviceProvider) : IAsyncDisp
{
try
{
if (Status == ActionStatus.INITIALIZING)
if (IsCancelAction)
{
Logger?.Info($"Thực hiện action {Type}");
Status = ActionStatus.RUNNING;
await StartAction();
await StopAction();
}
else if (Status == ActionStatus.RUNNING)
else
{
await ExecuteAction();
}
else if (Status == ActionStatus.PAUSED)
{
await PauseAction();
}
else if (Status == ActionStatus.WAITING && IsPaused)
{
await ResumeAction();
if (Status == ActionStatus.INITIALIZING)
{
Logger?.Info($"Thực hiện action {Type}");
Status = ActionStatus.RUNNING;
await StartAction();
}
else if (Status == ActionStatus.RUNNING)
{
await ExecuteAction();
}
else if (Status == ActionStatus.PAUSED)
{
await PauseAction();
}
else if (Status == ActionStatus.WAITING && IsPaused)
{
await ResumeAction();
}
}
if (IsCompleted)
{
await Dispose();
Dispose();
}
}
catch (Exception ex)
@ -171,16 +188,11 @@ public abstract class RobotAction(IServiceProvider serviceProvider) : IAsyncDisp
}
}
private Task Dispose()
public void Dispose()
{
if (!IsCompleted) StopAction();
ActionTimer?.Dispose();
ActionTimer = null;
return Task.CompletedTask;
}
public async ValueTask DisposeAsync()
{
await Dispose();
GC.SuppressFinalize(this);
}
}

21
RobotApp/Services/Robot/Actions/RobotCancelOrderAction.cs
View File

@ -1,20 +1,37 @@
using RobotApp.Interfaces;
using RobotApp.VDA5050.State;
namespace RobotApp.Services.Robot.Actions;
public class RobotCancelOrderAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
{
private IOrder? RobotOrder;
private IInstantActions? RobotAction;
protected override Task StartAction()
{
Scope ??= ServiceProvider.CreateAsyncScope();
var RobotOrder = Scope.ServiceProvider.GetRequiredService<IOrder>();
RobotOrder = Scope.ServiceProvider.GetRequiredService<IOrder>();
RobotAction = Scope.ServiceProvider.GetRequiredService<IInstantActions>();
RobotOrder.StopOrder();
Status = VDA5050.State.ActionStatus.FINISHED;
RobotAction.StopOrderAction();
return base.StartAction();
}
protected override Task ExecuteAction()
{
if (RobotOrder is null || RobotAction is null)
{
Status = ActionStatus.FAILED;
ResultDescription = $"Không thể tìm thấy module quản lý {(RobotOrder is null ? "Order" : RobotAction is null ? "Action" : "")}";
}
else
{
if (string.IsNullOrEmpty(RobotOrder.OrderId) && !RobotAction.HasActionRunning)
{
Status = ActionStatus.FINISHED;
ResultDescription = AgvAction is null ? ResultDescription : AgvAction.ResultDescription;
}
}
return base.ExecuteAction();
}
}

4
RobotApp/Services/Robot/Actions/RobotLiftDownAction.cs
View File

@ -2,11 +2,11 @@
public class RobotLiftDownAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
{
protected override Task StartAction()
protected override async Task StartAction()
{
await Task.Delay(2000);
Status = VDA5050.State.ActionStatus.FINISHED;
ResultDescription = AgvAction is null ? ResultDescription : AgvAction.ResultDescription;
return base.StartAction();
}
protected override Task ExecuteAction()

4
RobotApp/Services/Robot/Actions/RobotLiftUpAction.cs
View File

@ -2,11 +2,11 @@
public class RobotLiftUpAction(IServiceProvider ServiceProvider) : RobotAction(ServiceProvider)
{
protected override Task StartAction()
protected override async Task StartAction()
{
await Task.Delay(2000);
Status = VDA5050.State.ActionStatus.FINISHED;
ResultDescription = AgvAction is null ? ResultDescription : AgvAction.ResultDescription;
return base.StartAction();
}
protected override Task ExecuteAction()

1
RobotApp/Services/Robot/Actions/RobotMutedBaseOffAction.cs
View File

@ -9,6 +9,7 @@ public class RobotMutedBaseOffAction(IServiceProvider ServiceProvider) : RobotAc
Scope ??= ServiceProvider.CreateAsyncScope();
var RobotSafety = Scope.ServiceProvider.GetRequiredService<ISafety>();
RobotSafety.SetMutedBase(false);
Status = VDA5050.State.ActionStatus.FINISHED;
return base.StartAction();
}

1
RobotApp/Services/Robot/Actions/RobotMutedBaseOnAction.cs
View File

@ -9,6 +9,7 @@ public class RobotMutedBaseOnAction(IServiceProvider ServiceProvider) : RobotAct
Scope ??= ServiceProvider.CreateAsyncScope();
var RobotSafety = Scope.ServiceProvider.GetRequiredService<ISafety>();
RobotSafety.SetMutedBase(true);
Status = VDA5050.State.ActionStatus.FINISHED;
return base.StartAction();
}

1
RobotApp/Services/Robot/Actions/RobotMutedLoadOffAction.cs
View File

@ -9,6 +9,7 @@ public class RobotMutedLoadOffAction(IServiceProvider ServiceProvider) : RobotAc
Scope ??= ServiceProvider.CreateAsyncScope();
var RobotSafety = Scope.ServiceProvider.GetRequiredService<ISafety>();
RobotSafety.SetMutedLoad(false);
Status = VDA5050.State.ActionStatus.FINISHED;
return base.StartAction();
}

1
RobotApp/Services/Robot/Actions/RobotMutedLoadOnAction.cs
View File

@ -9,6 +9,7 @@ public class RobotMutedLoadOnAction(IServiceProvider ServiceProvider) : RobotAct
Scope ??= ServiceProvider.CreateAsyncScope();
var RobotSafety = Scope.ServiceProvider.GetRequiredService<ISafety>();
RobotSafety.SetMutedLoad(true);
Status = VDA5050.State.ActionStatus.FINISHED;
return base.StartAction();
}

26
RobotApp/Services/Robot/RobotActionController.cs
View File

@ -20,12 +20,14 @@ public class RobotActionController(Logger<RobotActionController> Logger, RobotAc
})];
public bool HasActionRunning => !ActionQueue.IsEmpty || Actions.Values.Any(a => !a.IsCompleted);
private readonly ConcurrentDictionary<string, RobotAction> Actions = [];
private readonly Dictionary<string, RobotAction> Actions = [];
private readonly ConcurrentQueue<(ActionScopes scope, VDA5050.InstantAction.Action action)> ActionQueue = [];
private WatchTimer<RobotActionController>? HandlerTimer;
private const int HandlerInterval = 200;
public RobotAction? this[string actionId] => Actions.TryGetValue(actionId, out RobotAction? action) && action is not null ? action : null;
public void AddInstantAction(VDA5050.InstantAction.Action[] actions)
{
foreach (var action in actions)
@ -42,7 +44,7 @@ public class RobotActionController(Logger<RobotActionController> Logger, RobotAc
}
}
public void StartOrderAction(string actionId, bool wait)
public void StartOrderAction(string actionId, bool wait = false)
{
if (Actions.TryGetValue(actionId, out RobotAction? robotAction) && robotAction is not null)
{
@ -54,6 +56,18 @@ public class RobotActionController(Logger<RobotActionController> Logger, RobotAc
}
}
public void StopOrderAction(string actionId = "")
{
if(string.IsNullOrEmpty(actionId))
{
foreach (var action in Actions.Values)
{
if (!action.IsCompleted) action.Cancel();
}
}
else if (Actions.TryGetValue(actionId, out RobotAction? robotAction) && robotAction is not null) robotAction.Cancel();
}
public void PauseActions()
{
foreach(var action in Actions.Values)
@ -78,7 +92,7 @@ public class RobotActionController(Logger<RobotActionController> Logger, RobotAc
{
if (ActionQueue.TryDequeue(out var result))
{
if (Actions.ContainsKey(result.action.ActionId)) return;
if (Actions.ContainsKey(result.action.ActionId)) continue;
if (Enum.TryParse(result.action.ActionType, out ActionType actionType))
{
var robotAction = ActionStorage[actionType];
@ -87,7 +101,7 @@ public class RobotActionController(Logger<RobotActionController> Logger, RobotAc
var init = robotAction.Initialize(result.scope, result.action);
if (init)
{
Actions.TryAdd(result.action.ActionId, robotAction);
Actions.Add(result.action.ActionId, robotAction);
if(result.scope == ActionScopes.INSTANT) robotAction.Start();
}
}
@ -113,6 +127,10 @@ public class RobotActionController(Logger<RobotActionController> Logger, RobotAc
public void ClearInstantActions()
{
ActionQueue.Clear();
foreach (var action in Actions.Values)
{
action.Dispose();
}
Actions.Clear();
}

13
RobotApp/Services/Robot/RobotConfiguration.cs
View File

@ -1,5 +1,7 @@
using RobotApp.Common.Shares.Enums;
using RobotApp.Interfaces;
using RobotApp.Services.Robot.Simulation;
using RobotApp.VDA5050.State;
namespace RobotApp.Services.Robot;
@ -22,6 +24,15 @@ public class RobotConfiguration
public byte PLCUnitId { get; set; } = 1;
public bool IsSimulation { get; set; } = true;
public SimulationModel SimulationModel { get; set; } = new();
public string XlocAddress { get; set; } = "http://192.168.195.56:50050";
public Dictionary<SafetySpeed, double> SafetySpeedMap = new ()
{
{ SafetySpeed.Very_Slow, 0.15},
{ SafetySpeed.Slow, 0.3},
{ SafetySpeed.Normal, 0.6},
{ SafetySpeed.Medium, 0.9},
{ SafetySpeed.Optimal, 1.2},
{ SafetySpeed.Fast, 1.5},
{ SafetySpeed.Very_Fast, 1.9},
};
}

25
RobotApp/Services/Robot/RobotController.cs
View File

@ -101,13 +101,32 @@ public partial class RobotController(IOrder OrderManager,
{
if(StateManager.HasState(AutoStateType.Executing.ToString()))
{
if(StateManager.HasState(ExecutingStateType.Moving.ToString())) OrderManager.PauseOrder();
else if(StateManager.HasState(ExecutingStateType.ACT.ToString())) ActionManager.PauseActions();
if (StateManager.HasState(ExecutingStateType.Moving.ToString()))
{
OrderManager.PauseOrder();
StateManager.TransitionTo(AutoStateType.Paused);
}
else if (StateManager.HasState(ExecutingStateType.ACT.ToString()))
{
ActionManager.PauseActions();
StateManager.TransitionTo(AutoStateType.Paused);
}
}
}
public void Resume()
{
if(StateManager.HasState(AutoStateType.Paused.ToString()))
{
StateManager.TransitionTo(AutoStateType.Executing);
if (StateManager.HasState(ExecutingStateType.Moving.ToString()))
{
OrderManager.ResumeOrder();
}
else if (StateManager.HasState(ExecutingStateType.ACT.ToString()))
{
ActionManager.ResumeActions();
}
}
}
}

8
RobotApp/Services/Robot/RobotFactsheet.cs
View File

@ -241,7 +241,7 @@ public class RobotFactsheet(RobotConnection RobotConnection, RobotConfiguration
ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()],
ActionParameters = [],
ResultDescription = "Robot đã bật chế độ muted base.",
BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString()],
BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString(), BlockingType.HARD.ToString()],
};
public readonly static AgvAction MutedBaseOff = new()
@ -251,7 +251,7 @@ public class RobotFactsheet(RobotConnection RobotConnection, RobotConfiguration
ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()],
ActionParameters = [],
ResultDescription = "Robot đã tắt chế độ muted base.",
BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString()],
BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString(), BlockingType.HARD.ToString()],
};
public readonly static AgvAction MutedLoadOn = new()
@ -261,7 +261,7 @@ public class RobotFactsheet(RobotConnection RobotConnection, RobotConfiguration
ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()],
ActionParameters = [],
ResultDescription = "Robot đã bật chế độ muted load.",
BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString()],
BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString(), BlockingType.HARD.ToString()],
};
public readonly static AgvAction MutedLoadOff = new()
@ -271,7 +271,7 @@ public class RobotFactsheet(RobotConnection RobotConnection, RobotConfiguration
ActionScopes = [ActionScopes.INSTANT.ToString(), ActionScopes.NODE.ToString()],
ActionParameters = [],
ResultDescription = "Robot đã tắt chế độ muted load.",
BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString()],
BlockingTypes = [BlockingType.NONE.ToString(), BlockingType.SOFT.ToString(), BlockingType.HARD.ToString()],
};
public readonly static AgvAction DockTo = new()

6
RobotApp/Services/Robot/RobotNavigation.cs
View File

@ -16,7 +16,6 @@ public class RobotNavigation(RobotConfiguration RobotConfiguration, IServiceProv
private SimulationNavigation? SimNavigation;
private readonly bool IsSimulation = RobotConfiguration.IsSimulation;
public event Action? OnNavigationStateChanged;
public event Action? OnNavigationFinished;
public void CancelMovement()
@ -85,4 +84,9 @@ public class RobotNavigation(RobotConfiguration RobotConfiguration, IServiceProv
OnNavigationFinished?.Invoke();
if(SimNavigation is not null) SimNavigation.OnNavigationFinished -= NavigationFinished;
}
public void SetSpeed(double speed)
{
return;
}
}

181
RobotApp/Services/Robot/RobotOrderController.cs
View File

@ -1,10 +1,11 @@
using Microsoft.AspNetCore.Components;
using RobotApp.Common.Shares.Enums;
using RobotApp.Common.Shares.Enums;
using RobotApp.Interfaces;
using RobotApp.Services.Exceptions;
using RobotApp.Services.State;
using RobotApp.VDA5050.InstantAction;
using RobotApp.VDA5050.Order;
using RobotApp.VDA5050.State;
using System.Collections.Concurrent;
using Action = RobotApp.VDA5050.InstantAction.Action;
namespace RobotApp.Services.Robot;
@ -13,33 +14,15 @@ public class RobotOrderController(INavigation NavigationManager,
ILocalization Localization,
IInstantActions ActionManager,
IError ErrorManager,
ISafety SafetyManager,
RobotStateMachine StateManager,
RobotConfiguration RobotConfiguration,
Logger<RobotOrderController> Logger) : IOrder
{
public string OrderId { get; private set; } = string.Empty;
public int OrderUpdateId { get; private set; }
public NodeState[] NodeStates => [.. Nodes.Select(n => new NodeState
{
NodeId = n.NodeId,
Released = n.Released,
SequenceId = n.SequenceId,
NodeDescription = n.NodeDescription,
NodePosition = new()
{
X = n.NodePosition.X,
Y = n.NodePosition.Y,
Theta = n.NodePosition.Theta,
MapId = n.NodePosition.MapId
}
})];
public EdgeState[] EdgeStates => [.. Edges.Select(e => new EdgeState
{
EdgeId = e.EdgeId,
Released = e.Released,
EdgeDescription = e.EdgeDescription,
SequenceId = e.SequenceId,
Trajectory = e.Trajectory
})];
public NodeState[] NodeStates { get; private set; } = [];
public EdgeState[] EdgeStates { get; private set; } = [];
public string LastNodeId => LastNode is null ? "" : LastNode.NodeId;
public int LastNodeSequenceId => LastNode is null ? 0 : LastNode.SequenceId;
@ -47,13 +30,20 @@ public class RobotOrderController(INavigation NavigationManager,
private WatchTimer<RobotOrderController>? OrderTimer;
private readonly Dictionary<string, Action[]> OrderActions = [];
private readonly ConcurrentQueue<Action> ActionWaitingRunning = [];
private OrderMsg? NewOrder;
private Node[] Nodes = [];
private Edge[] Edges = [];
private Node? CurrentBaseNode;
private Node? LastNode;
private readonly Lock LockObject = new();
private OrderMsg? NewOrder;
private bool IsCancelOrder = false;
private bool IsActionRunning = false;
private bool IsWaitingPaused = false;
private Action? ActionHard = null;
public void UpdateOrder(OrderMsg order)
{
@ -66,8 +56,7 @@ public class RobotOrderController(INavigation NavigationManager,
public void StopOrder()
{
NavigationManager.CancelMovement();
NavigationFinished();
if (!string.IsNullOrEmpty(OrderId) || OrderTimer is not null) IsCancelOrder = true;
}
public void PauseOrder()
@ -99,9 +88,9 @@ public class RobotOrderController(INavigation NavigationManager,
foreach (var node in Nodes)
{
var distance = Localization.DistanceTo(node.NodePosition.X, node.NodePosition.Y);
if(distance <= node.NodePosition.AllowedDeviationXY)
if (distance <= node.NodePosition.AllowedDeviationXY)
{
if(distance < minDistance)
if (distance < minDistance)
{
minDistance = distance;
inNode = node;
@ -114,43 +103,89 @@ public class RobotOrderController(INavigation NavigationManager,
private void NavigationFinished()
{
HandleOrderStop();
SafetyManager.OnSafetySpeedChanged -= OnSafetySpeedChanged;
OrderId = string.Empty;
OrderUpdateId = 0;
OrderActions.Clear();
ActionWaitingRunning.Clear();
CurrentBaseNode = null;
Nodes = [];
Edges = [];
StateManager.TransitionTo(AutoStateType.Idle);
}
private void HanleNewOrder(OrderMsg order)
private void OnSafetySpeedChanged(SafetySpeed safetySpeed)
{
double speed = 0.15;
if (RobotConfiguration.SafetySpeedMap.TryGetValue(safetySpeed, out double safeSpeed)) speed = safeSpeed;
NavigationManager.SetSpeed(speed);
}
private void UpdateState()
{
NodeStates = [.. Nodes.Select(n => new NodeState
{
NodeId = n.NodeId,
Released = n.Released,
SequenceId = n.SequenceId,
NodeDescription = n.NodeDescription,
NodePosition = new()
{
X = n.NodePosition.X,
Y = n.NodePosition.Y,
Theta = n.NodePosition.Theta,
MapId = n.NodePosition.MapId
}
})];
EdgeStates = [.. Edges.Select(e => new EdgeState
{
EdgeId = e.EdgeId,
Released = e.Released,
EdgeDescription = e.EdgeDescription,
SequenceId = e.SequenceId,
Trajectory = e.Trajectory
})];
}
private void HandleNewOrder(OrderMsg order)
{
if (NavigationManager.State != NavigationState.Idle) throw new OrderException(RobotErrors.Error1012(NavigationManager.State));
OrderActions.Clear();
for (int i = 0; i < order.Nodes.Length; i++)
{
if (order.Nodes[i].Actions is not null && order.Nodes[i].Actions.Length > 0)
{
foreach (var item in order.Nodes[i].Actions)
{
item.ActionDescription += $"\n NodeId: {order.Nodes[i].NodeId}";
item.ActionDescription += $".On NodeId: {order.Nodes[i].NodeId}";
}
OrderActions.Add(order.Nodes[i].NodeId, order.Nodes[i].Actions);
if (OrderActions.TryGetValue(order.Nodes[i].NodeId, out Action[]? actions) && actions is not null)
{
actions = [.. actions, .. order.Nodes[i].Actions];
}
else OrderActions.Add(order.Nodes[i].NodeId, order.Nodes[i].Actions);
}
if (i < order.Nodes.Length - 1 && order.Edges[i] is not null && order.Edges[i].Length > 0)
if (i < order.Nodes.Length - 1 && order.Edges[i].Actions is not null && order.Edges[i].Actions.Length > 0)
{
foreach (var item in order.Edges[i].Actions)
{
item.ActionDescription += $"\n NodeId: {order.Nodes[i].NodeId}";
item.ActionDescription += $".On NodeId: {order.Nodes[i].NodeId}";
}
OrderActions.TryAdd(order.Nodes[i].NodeId, order.Edges[i].Actions);
if (OrderActions.TryGetValue(order.Nodes[i].NodeId, out Action[]? actions) && actions is not null)
{
actions = [.. actions, .. order.Edges[i].Actions];
}
else OrderActions.Add(order.Nodes[i].NodeId, order.Edges[i].Actions);
}
if (order.Nodes[i].SequenceId != i) throw new OrderException(RobotErrors.Error1010(order.Nodes[i].NodeId, order.Nodes[i].SequenceId, i));
if (i < order.Nodes.Length - 1 && order.Edges[i].SequenceId != i) throw new OrderException(RobotErrors.Error1011(order.Edges[i].EdgeId, order.Edges[i].SequenceId, i));
if (order.Nodes[i].Released) CurrentBaseNode = order.Nodes[i];
}
ActionManager.ClearInstantActions();
SafetyManager.OnSafetySpeedChanged += OnSafetySpeedChanged;
if (OrderActions.Count > 0) ActionManager.AddOrderActions([.. OrderActions.Values.SelectMany(a => a)]);
NavigationManager.Move(order.Nodes, order.Edges);
NavigationManager.OnNavigationFinished += NavigationFinished;
@ -159,7 +194,8 @@ public class RobotOrderController(INavigation NavigationManager,
Nodes = order.Nodes;
Edges = order.Edges;
if (CurrentBaseNode is not null) NavigationManager.UpdateOrder(CurrentBaseNode.NodeId);
if(StateManager.CurrentStateName != AutoStateType.Executing.ToString()) StateManager.TransitionTo(AutoStateType.Executing);
if (StateManager.CurrentStateName != AutoStateType.Executing.ToString()) StateManager.TransitionTo(AutoStateType.Executing);
UpdateState();
}
private bool IsNewPath()
@ -167,6 +203,18 @@ public class RobotOrderController(INavigation NavigationManager,
return true;
}
private void ClearLastNode()
{
if (LastNode is null) return;
var currentLastNodeIndex = Array.FindIndex(Nodes, n => n.NodeId == LastNode.NodeId.ToString());
if (currentLastNodeIndex != -1 && currentLastNodeIndex < Nodes.Length - 1)
{
Nodes = [.. Nodes.Skip(currentLastNodeIndex + 1)];
Edges = [.. Edges.Skip(currentLastNodeIndex + 1)];
UpdateState();
}
}
private void HandleUpdateOrder(OrderMsg order)
{
if (order.OrderId != OrderId) throw new OrderException(RobotErrors.Error1001(OrderId, order.OrderId));
@ -181,15 +229,66 @@ public class RobotOrderController(INavigation NavigationManager,
Nodes = order.Nodes;
Edges = order.Edges;
OrderUpdateId = order.OrderUpdateId;
ClearLastNode();
}
private void HandleOrder()
{
if (IsCancelOrder)
{
IsCancelOrder = false;
NavigationManager.CancelMovement();
NavigationFinished();
return;
}
var currentNode = GetCurrentNode();
if (currentNode is not null && currentNode.NodeId != LastNode?.NodeId)
{
LastNode = currentNode;
// xuử lí nếu gặp node mới
ClearLastNode();
if (OrderActions.TryGetValue(currentNode.NodeId, out Action[]? actions) && actions is not null && actions.Length > 0)
{
if (actions.Any(a => a.BlockingType == BlockingType.SOFT.ToString() || a.BlockingType == BlockingType.HARD.ToString()))
{
NavigationManager.Pause();
IsWaitingPaused = true;
}
foreach (var action in actions)
{
ActionWaitingRunning.Enqueue(action);
}
}
}
if (ActionHard is not null)
{
var robotAction = ActionManager[ActionHard.ActionId];
if (robotAction is null || (robotAction is not null && robotAction.IsCompleted))
{
NavigationManager.Resume();
ActionHard = null;
}
}
else
{
if (!ActionWaitingRunning.IsEmpty)
{
IsActionRunning = !IsWaitingPaused || (IsWaitingPaused && NavigationManager.State == NavigationState.Paused);
if (IsActionRunning)
{
if (ActionWaitingRunning.TryDequeue(out Action? action) && action is not null)
{
ActionManager.StartOrderAction(action.ActionId);
ActionHard = action.BlockingType == BlockingType.HARD.ToString() ? action : null;
}
}
}
else
{
if (IsWaitingPaused) IsWaitingPaused = false;
}
}
}
@ -199,7 +298,6 @@ public class RobotOrderController(INavigation NavigationManager,
{
if (NewOrder is not null)
{
Console.WriteLine("Has new Order");
OrderMsg NewOrderHandler;
lock (LockObject)
{
@ -211,12 +309,12 @@ public class RobotOrderController(INavigation NavigationManager,
if (NewOrderHandler.Edges.Length < 1) throw new OrderException(RobotErrors.Error1003(NewOrderHandler.Edges.Length));
if (NewOrderHandler.Edges.Length != NewOrderHandler.Nodes.Length - 1) throw new OrderException(RobotErrors.Error1004(NewOrderHandler.Nodes.Length, NewOrderHandler.Edges.Length));
if (string.IsNullOrEmpty(OrderId)) HanleNewOrder(NewOrderHandler);
if (string.IsNullOrEmpty(OrderId)) HandleNewOrder(NewOrderHandler);
else HandleUpdateOrder(NewOrderHandler);
}
HandleOrder();
}
catch (RobotExeption orEx )
catch (RobotExeption orEx)
{
if (orEx.Error is not null)
{
@ -230,4 +328,5 @@ public class RobotOrderController(INavigation NavigationManager,
Logger.Warning($"Lỗi khi xử lí Order: {ex.Message}");
}
}
}

1
RobotApp/Services/Robot/Simulation/Navigation/DifferentialNavigation.cs
View File

@ -68,6 +68,7 @@ public class DifferentialNavigation : SimulationNavigation
}
}
}
else if (NavState == NavigationState.Paused) VelocityController.Stop();
}
catch (Exception ex)
{

6
RobotApp/Services/Robot/Simulation/Navigation/SimulationNavigation.cs
View File

@ -16,7 +16,6 @@ public class SimulationNavigation : INavigation, IDisposable
public double VelocityY => Visualization.Vy;
public double Omega => Visualization.Omega;
public event Action? OnNavigationStateChanged;
public event Action? OnNavigationFinished;
protected NavigationState NavState = NavigationState.Idle;
@ -182,6 +181,11 @@ public class SimulationNavigation : INavigation, IDisposable
throw new NotImplementedException();
}
public void SetSpeed(double speed)
{
throw new NotImplementedException();
}
public void Dispose()
{
HandleNavigationStop();

6
RobotApp/Services/State/RobotStateMachine.cs
View File

@ -1,10 +1,12 @@
using RobotApp.Common.Shares.Enums;
using System.Collections.Concurrent;
namespace RobotApp.Services.State;
public record RobotStateMachine(Logger<RobotStateMachine> Logger) : IDisposable
{
private readonly Dictionary<Type, Dictionary<Enum, IRobotState>> StateRegistry = [];
private readonly Lock StateLock = new();
private readonly ConcurrentDictionary<Type, Dictionary<Enum, IRobotState>> StateRegistry = [];
public IRobotState CurrentState { get; private set; } = null!;
public event Action<IRobotState?, IRobotState>? OnStateChanged;
@ -12,8 +14,6 @@ public record RobotStateMachine(Logger<RobotStateMachine> Logger) : IDisposable
public string CurrentStateName => CurrentState.Name.ToString();
public string RootStateName => GetRootStateName();
private readonly Lock StateLock = new();
public void InitializeHierarchyStates()
{
if (IsInitialized) return;