518 lines
20 KiB
C#
518 lines
20 KiB
C#
using RobotApp.Common.Shares.Dtos;
|
|
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 System.Data;
|
|
using Action = RobotApp.VDA5050.InstantAction.Action;
|
|
|
|
namespace RobotApp.Services.Robot;
|
|
|
|
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 { 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;
|
|
public (NodeState[], EdgeStateDto[]) CurrentPath => GetCurrentPath();
|
|
|
|
private const int CycleHandlerMilliseconds = 200;
|
|
private WatchTimer<RobotOrderController>? OrderTimer;
|
|
|
|
private readonly Dictionary<string, Action[]> OrderActions = [];
|
|
private readonly ConcurrentQueue<Action> ActionWaitingRunning = [];
|
|
private List<Action> FinalAction = [];
|
|
|
|
private OrderMsg? NewOrder;
|
|
private Node[] Nodes = [];
|
|
private VDA5050.Order.Edge[] Edges = [];
|
|
private Node? CurrentBaseNode;
|
|
private Node? LastNode;
|
|
private Node[] NewOrderNodes = [];
|
|
private VDA5050.Order.Edge[] NewOrderEdges = [];
|
|
|
|
private readonly Lock LockObject = new();
|
|
|
|
private bool IsCancelOrder = false;
|
|
private bool IsActionRunning = false;
|
|
private bool IsWaitingPaused = false;
|
|
private bool IsNavigationFinished = false;
|
|
private bool IsOneceNode = false;
|
|
private Action? ActionHard = null;
|
|
|
|
public void UpdateOrder(OrderMsg order)
|
|
{
|
|
lock (LockObject)
|
|
{
|
|
NewOrder = order;
|
|
}
|
|
if (OrderTimer is null) HandleOrderStart();
|
|
}
|
|
|
|
public void StopOrder()
|
|
{
|
|
if (NodeStates.Length > 0 || OrderTimer is not null) IsCancelOrder = true;
|
|
}
|
|
|
|
public void PauseOrder()
|
|
{
|
|
NavigationManager.Pause();
|
|
}
|
|
|
|
public void ResumeOrder()
|
|
{
|
|
NavigationManager.Resume();
|
|
}
|
|
|
|
private void HandleOrderStart()
|
|
{
|
|
OrderTimer = new(CycleHandlerMilliseconds, OrderHandler, Logger);
|
|
OrderTimer.Start();
|
|
}
|
|
|
|
private void HandleOrderStop()
|
|
{
|
|
OrderTimer?.Dispose();
|
|
OrderTimer = null;
|
|
OrderActions.Clear();
|
|
ActionWaitingRunning.Clear();
|
|
CurrentBaseNode = null;
|
|
Nodes = [];
|
|
Edges = [];
|
|
UpdateState();
|
|
SafetyManager.OnSafetySpeedChanged -= OnSafetySpeedChanged;
|
|
NavigationManager.OnNavigationFinished -= NavigationFinished;
|
|
StateManager.TransitionTo(AutoStateType.Idle);
|
|
}
|
|
|
|
private Node? GetCurrentNode()
|
|
{
|
|
Node? inNode = null;
|
|
double minDistance = double.MaxValue;
|
|
foreach (var node in Nodes)
|
|
{
|
|
var distance = Localization.DistanceTo(node.NodePosition.X, node.NodePosition.Y);
|
|
var nodeMin = node.NodePosition.AllowedDeviationXY == 0.0 ? 0.5 : node.NodePosition.AllowedDeviationXY;
|
|
if (distance <= nodeMin)
|
|
{
|
|
if (distance < minDistance && node.NodeId != Nodes[^1].NodeId)
|
|
{
|
|
minDistance = distance;
|
|
inNode = node;
|
|
}
|
|
}
|
|
}
|
|
return inNode;
|
|
}
|
|
|
|
private void NavigationFinished()
|
|
{
|
|
IsNavigationFinished = true;
|
|
}
|
|
|
|
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 (order.OrderId == OrderId)
|
|
{
|
|
if (order.OrderUpdateId < OrderUpdateId) throw new OrderException(RobotErrors.Error1018(OrderUpdateId, order.OrderUpdateId));
|
|
if (order.OrderUpdateId == OrderUpdateId) return;
|
|
if (order.Nodes[0].NodeId != LastNodeId)
|
|
{
|
|
throw new OrderException(RobotErrors.Error1016(LastNodeId, order.Nodes[0].NodeId));
|
|
}
|
|
if (order.Nodes[0].SequenceId != LastNodeSequenceId)
|
|
{
|
|
throw new OrderException(RobotErrors.Error1017(LastNodeSequenceId, order.Nodes[0].SequenceId));
|
|
}
|
|
}
|
|
|
|
OrderActions.Clear();
|
|
ActionManager.ClearInstantActions();
|
|
LastNode = null;
|
|
FinalAction = [];
|
|
IsNavigationFinished = false;
|
|
IsCancelOrder = false;
|
|
IsActionRunning = false;
|
|
IsWaitingPaused = false;
|
|
ActionHard = null;
|
|
|
|
for (int i = 0; i < order.Edges.Length; i++)
|
|
{
|
|
if (order.Edges[i].Trajectory is null)
|
|
{
|
|
order.Edges[i].Trajectory = new Trajectory()
|
|
{
|
|
Degree = 1,
|
|
ControlPoints = [new ControlPoint() { X = 0, Y = 0 }]
|
|
};
|
|
}
|
|
}
|
|
|
|
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 += $".On NodeId: {order.Nodes[i].NodeId}";
|
|
}
|
|
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].Actions is not null && order.Edges[i].Actions.Length > 0)
|
|
{
|
|
foreach (var item in order.Edges[i].Actions)
|
|
{
|
|
item.ActionDescription += $".On NodeId: {order.Nodes[i].NodeId}";
|
|
}
|
|
|
|
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].Released) CurrentBaseNode = order.Nodes[i];
|
|
}
|
|
SafetyManager.OnSafetySpeedChanged += OnSafetySpeedChanged;
|
|
|
|
if (OrderActions.TryGetValue(order.Nodes[^1].NodeId, out Action[]? finalactions) && finalactions is not null && finalactions.Length > 0) FinalAction = [.. finalactions];
|
|
|
|
if (OrderActions.Count > 0) ActionManager.AddOrderActions([.. OrderActions.Values.SelectMany(a => a)]);
|
|
|
|
if (order.Nodes.Length > 1 && order.Edges.Length >= 0)
|
|
{
|
|
NavigationManager.Move(order.Nodes, order.Edges);
|
|
NavigationManager.OnNavigationFinished += NavigationFinished;
|
|
}
|
|
else IsOneceNode = true;
|
|
OrderId = order.OrderId;
|
|
OrderUpdateId = order.OrderUpdateId;
|
|
Nodes = order.Nodes;
|
|
Edges = order.Edges;
|
|
NewOrderNodes = order.Nodes;
|
|
NewOrderEdges = order.Edges;
|
|
if (CurrentBaseNode is not null && CurrentBaseNode.NodeId != Nodes[0].NodeId && Nodes.Length > 1) NavigationManager.UpdateOrder(CurrentBaseNode.NodeId);
|
|
if (StateManager.CurrentStateName != AutoStateType.Executing.ToString()) StateManager.TransitionTo(AutoStateType.Executing);
|
|
UpdateState();
|
|
}
|
|
|
|
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();
|
|
}
|
|
}
|
|
|
|
public void HandleUpdateOrder(OrderMsg order)
|
|
{
|
|
if (order.OrderId != OrderId) throw new OrderException(RobotErrors.Error1001(OrderId, order.OrderId));
|
|
if (order.OrderUpdateId <= OrderUpdateId) return;
|
|
|
|
if (CurrentBaseNode is not null && order.Nodes[0].NodeId != CurrentBaseNode.NodeId)
|
|
{
|
|
throw new OrderException(RobotErrors.Error1016(LastNodeId, order.Nodes[0].NodeId));
|
|
}
|
|
if (CurrentBaseNode is not null && order.Nodes[0].SequenceId != CurrentBaseNode.SequenceId)
|
|
{
|
|
throw new OrderException(RobotErrors.Error1017(LastNodeSequenceId, order.Nodes[0].SequenceId));
|
|
}
|
|
|
|
var lastBastNode = order.Nodes.Last(n => n.Released);
|
|
if (lastBastNode is not null && lastBastNode.NodeId != CurrentBaseNode?.NodeId)
|
|
{
|
|
CurrentBaseNode = lastBastNode;
|
|
NavigationManager.UpdateOrder(CurrentBaseNode.NodeId);
|
|
}
|
|
Nodes = order.Nodes;
|
|
Edges = order.Edges;
|
|
OrderUpdateId = order.OrderUpdateId;
|
|
ClearLastNode();
|
|
}
|
|
|
|
private void HandleOrder()
|
|
{
|
|
if (Nodes.Length <= 0) return;
|
|
if (IsCancelOrder)
|
|
{
|
|
NavigationManager.CancelMovement();
|
|
}
|
|
|
|
if (IsNavigationFinished || IsOneceNode)
|
|
{
|
|
if (IsCancelOrder && !ActionManager.HasActionRunning)
|
|
{
|
|
HandleOrderStop();
|
|
return;
|
|
}
|
|
|
|
if (FinalAction.Count > 0)
|
|
{
|
|
var action = FinalAction[0];
|
|
var robotAction = ActionManager[action.ActionId];
|
|
if (robotAction is null)
|
|
{
|
|
FinalAction.Remove(action);
|
|
return;
|
|
}
|
|
if (robotAction.IsCompleted)
|
|
if (robotAction.Status == ActionStatus.WAITING) ActionManager.StartOrderAction(action.ActionId);
|
|
}
|
|
else
|
|
{
|
|
LastNode = Nodes[^1]; // ddoanj nay phai sua lai kien tra xem co toi node cuoi that khong
|
|
HandleOrderStop();
|
|
return;
|
|
}
|
|
}
|
|
|
|
var currentNode = GetCurrentNode();
|
|
if (currentNode is not null && currentNode.NodeId != LastNode?.NodeId)
|
|
{
|
|
LastNode = currentNode;
|
|
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) return;
|
|
if (robotAction is not null && robotAction.IsCompleted) 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)
|
|
{
|
|
var robotAction = ActionManager[action.ActionId];
|
|
if (robotAction is null)
|
|
{
|
|
ActionWaitingRunning.Enqueue(action);
|
|
return;
|
|
}
|
|
ActionManager.StartOrderAction(action.ActionId);
|
|
ActionHard = action.BlockingType == BlockingType.HARD.ToString() ? action : null;
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (IsWaitingPaused)
|
|
{
|
|
IsWaitingPaused = false;
|
|
NavigationManager.Resume();
|
|
if (CurrentBaseNode is not null && CurrentBaseNode.NodeId != Nodes[0].NodeId && Nodes.Length > 1) NavigationManager.UpdateOrder(CurrentBaseNode.NodeId);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
private void OrderHandler()
|
|
{
|
|
try
|
|
{
|
|
if (NewOrder is not null)
|
|
{
|
|
OrderMsg NewOrderHandler;
|
|
lock (LockObject)
|
|
{
|
|
NewOrderHandler = NewOrder;
|
|
NewOrder = null;
|
|
}
|
|
|
|
if (NewOrderHandler.Nodes.Length < 1) throw new OrderException(RobotErrors.Error1002(NewOrderHandler.Nodes.Length));
|
|
if (NewOrderHandler.Edges.Length != NewOrderHandler.Nodes.Length - 1) throw new OrderException(RobotErrors.Error1004(NewOrderHandler.Nodes.Length, NewOrderHandler.Edges.Length));
|
|
|
|
if (NodeStates.Length != 0 || EdgeStates.Length != 0) HandleUpdateOrder(NewOrderHandler);
|
|
else
|
|
{
|
|
Node startNode = NewOrderHandler.Nodes[0];
|
|
var nodeDeviation = startNode.NodePosition.AllowedDeviationXY == 0.0 ? NewOrderHandler.Nodes.Length == 1 ? 0.3 : 0.5 : startNode.NodePosition.AllowedDeviationXY;
|
|
var distance = Localization.DistanceTo(startNode.NodePosition.X, startNode.NodePosition.Y);
|
|
if (distance > nodeDeviation) throw new OrderException(RobotErrors.Error1019());
|
|
|
|
if (NewOrderHandler.Nodes.Length > 1)
|
|
{
|
|
Node endNode = NewOrderHandler.Nodes[^1];
|
|
nodeDeviation = endNode.NodePosition.AllowedDeviationXY == 0.0 ? 0.2 : endNode.NodePosition.AllowedDeviationXY;
|
|
distance = Localization.DistanceTo(endNode.NodePosition.X, endNode.NodePosition.Y);
|
|
if (distance < nodeDeviation) throw new OrderException(RobotErrors.Error1020());
|
|
}
|
|
HandleNewOrder(NewOrderHandler);
|
|
}
|
|
}
|
|
HandleOrder();
|
|
}
|
|
catch (RobotExeption orEx)
|
|
{
|
|
if (orEx.Error is not null)
|
|
{
|
|
ErrorManager.AddError(orEx.Error, TimeSpan.FromSeconds(10));
|
|
Logger.Warning($"Lỗi khi xử lí Order: {orEx.Error.ErrorDescription}");
|
|
if (Nodes.Length == 0) HandleOrderStop();
|
|
}
|
|
else Logger.Warning($"Lỗi khi xử lí Order: {orEx.Message}");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Logger.Warning($"Lỗi khi xử lí Order: {ex.Message}");
|
|
}
|
|
}
|
|
|
|
private EdgeStateDto[] SplitChecking(Node lastNode, Node nearLastNode, VDA5050.Order.Edge edge)
|
|
{
|
|
List<EdgeStateDto> pathEdges = [];
|
|
var splitStartPath = RobotPathPlanner.PathSplit([lastNode, nearLastNode], [edge], 0.5);
|
|
if (splitStartPath is not null && splitStartPath.Length > 0)
|
|
{
|
|
int index = 0;
|
|
double minDistance = double.MaxValue;
|
|
for (int i = 0; i < splitStartPath.Length; i++)
|
|
{
|
|
var distance = Math.Sqrt(Math.Pow(Localization.X - splitStartPath[i].NodePosition.X, 2) +
|
|
Math.Pow(Localization.Y - splitStartPath[i].NodePosition.Y, 2));
|
|
if (distance < minDistance)
|
|
{
|
|
minDistance = distance;
|
|
index = i;
|
|
}
|
|
}
|
|
if (edge.Trajectory is null || edge.Trajectory.Degree == 1)
|
|
{
|
|
pathEdges.Add(new()
|
|
{
|
|
StartX = splitStartPath[index].NodePosition.X,
|
|
StartY = splitStartPath[index].NodePosition.Y,
|
|
EndX = nearLastNode.NodePosition.X,
|
|
EndY = nearLastNode.NodePosition.Y,
|
|
Degree = 1,
|
|
});
|
|
}
|
|
else
|
|
{
|
|
for (int i = index; i < splitStartPath.Length - 1; i++)
|
|
{
|
|
pathEdges.Add(new()
|
|
{
|
|
StartX = splitStartPath[i].NodePosition.X,
|
|
StartY = splitStartPath[i].NodePosition.Y,
|
|
EndX = splitStartPath[i + 1].NodePosition.X,
|
|
EndY = splitStartPath[i + 1].NodePosition.Y,
|
|
Degree = 1,
|
|
});
|
|
}
|
|
}
|
|
}
|
|
return [.. pathEdges];
|
|
}
|
|
|
|
private (NodeState[], EdgeStateDto[]) GetCurrentPath()
|
|
{
|
|
if (NodeStates.Length == 0 && EdgeStates.Length == 0) return ([], []);
|
|
|
|
List<EdgeStateDto> pathEdges = [];
|
|
var lastNodeIndex = Array.FindIndex(NewOrderNodes, n => n.NodeId == LastNodeId);
|
|
lastNodeIndex = lastNodeIndex != -1 ? lastNodeIndex : 0;
|
|
if (lastNodeIndex < NewOrderNodes.Length - 1) pathEdges = [.. SplitChecking(NewOrderNodes[lastNodeIndex], NewOrderNodes[lastNodeIndex + 1], NewOrderEdges[lastNodeIndex])];
|
|
if (lastNodeIndex < NewOrderNodes.Length - 2)
|
|
{
|
|
var nodes = NewOrderNodes.ToList().GetRange(lastNodeIndex + 1, NewOrderNodes.Length - lastNodeIndex - 1);
|
|
var edges = NewOrderEdges.ToList().GetRange(lastNodeIndex + 1, nodes.Count - 1);
|
|
for (int i = 0; i < nodes.Count - 1; i++)
|
|
{
|
|
if (edges[i] is null) return (NodeStates, [.. pathEdges]);
|
|
|
|
var trajectory = edges[i].Trajectory;
|
|
var controlPoints = trajectory?.ControlPoints;
|
|
|
|
|
|
pathEdges.Add(new()
|
|
{
|
|
StartX = nodes[i].NodePosition.X,
|
|
StartY = nodes[i].NodePosition.Y,
|
|
EndX = nodes[i + 1].NodePosition.X,
|
|
EndY = nodes[i + 1].NodePosition.Y,
|
|
ControlPoint1X = controlPoints is { Length: > 2 } ? controlPoints[1].X : 0,
|
|
ControlPoint1Y = controlPoints is { Length: > 2 } ? controlPoints[1].Y : 0,
|
|
ControlPoint2X = controlPoints is { Length: > 3 } ? controlPoints[2].X : 0,
|
|
ControlPoint2Y = controlPoints is { Length: > 3 } ? controlPoints[2].Y : 0,
|
|
Degree = trajectory is null ? 1 : trajectory.Degree,
|
|
});
|
|
}
|
|
}
|
|
return (NodeStates, [.. pathEdges]);
|
|
}
|
|
|
|
}
|