970 lines
40 KiB
C#
970 lines
40 KiB
C#
using RobotNet.VDA5050.Order;
|
|
using RobotNet.VDA5050.State;
|
|
using RobotNet.VDA5050.Type;
|
|
using RobotNet10.Common;
|
|
using RobotNet10.RobotApp.Client.Pages;
|
|
using RobotNet10.RobotApp.Interfaces;
|
|
using RobotNet10.RobotApp.Services.ConfigManager;
|
|
using RobotNet10.RobotApp.Services.Exceptions;
|
|
using RobotNet10.RobotApp.Services.State;
|
|
using System.Collections.Concurrent;
|
|
using System.Data;
|
|
using Action = RobotNet.VDA5050.InstantAction.Action;
|
|
|
|
namespace RobotNet10.RobotApp.Services.Robot;
|
|
|
|
public class RobotOrderController(INavigation NavigationManager,
|
|
ILocalization Localization,
|
|
IAction ActionManager,
|
|
IError ErrorManager,
|
|
IPlcController SafetyManager,
|
|
RobotStateMachine StateManager,
|
|
INavigationConfig NavigationConfig,
|
|
ILogger<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 bool NewBaseRequest { get; private set; }
|
|
public double DistanceSinceLastNode { get; private set; }
|
|
public bool IsPaused { get; private set; } = false;
|
|
|
|
private const int CycleHandlerMilliseconds = 100;
|
|
private WatchThread<RobotOrderController>? OrderTimer;
|
|
|
|
private readonly Dictionary<string, Action[]> OrderActions = []; // Node actions keyed by NodeId
|
|
private readonly ConcurrentQueue<Action> ActionWaitingRunning = [];
|
|
|
|
private OrderMsg? NewOrder;
|
|
private OrderMsg? _currentActiveOrder;
|
|
private Node[] Nodes = [];
|
|
private Edge[] Edges = [];
|
|
private Node? CurrentBaseNode;
|
|
private Node? LastNode;
|
|
private Edge? CurrentEdge; // Track current edge for EDGE action lifecycle
|
|
private readonly ConcurrentBag<string> RunningEdgeActionIds = []; // Track running EDGE action IDs (thread-safe)
|
|
|
|
private readonly Lock LockObject = new();
|
|
|
|
private bool IsCancelOrder = false;
|
|
private bool IsCancelSentToNavigation = false;
|
|
private bool IsActionRunning = false;
|
|
private bool IsWaitingPaused = false;
|
|
private bool IsNavigationFinished = false;
|
|
private bool HasNewOrder = false;
|
|
private Action? ActionHard = null;
|
|
private NavigationState NavState = NavigationState.None;
|
|
|
|
private double SafetySpeed = 0.0;
|
|
private double EdgeSpeed = 0.0;
|
|
private double CurrentSpeed = 0.0;
|
|
private Navigation.NavigationConfig? CachedNavConfig = null;
|
|
|
|
public void UpdateOrder(OrderMsg order)
|
|
{
|
|
bool shouldStart = false;
|
|
lock (LockObject)
|
|
{
|
|
NewOrder = order;
|
|
if (OrderTimer is null)
|
|
{
|
|
shouldStart = true;
|
|
}
|
|
}
|
|
if (shouldStart) HandleOrderStart();
|
|
}
|
|
|
|
public void StopOrder()
|
|
{
|
|
if (NodeStates.Length > 0 || OrderTimer is not null)
|
|
{
|
|
IsCancelOrder = true;
|
|
IsCancelSentToNavigation = false;
|
|
}
|
|
}
|
|
|
|
public void PauseOrder()
|
|
{
|
|
IsPaused = true;
|
|
NavigationManager.Pause();
|
|
ActionManager.PauseActions();
|
|
}
|
|
|
|
public void ResumeOrder()
|
|
{
|
|
IsPaused = false;
|
|
NavigationManager.Resume();
|
|
ActionManager.ResumeActions();
|
|
}
|
|
|
|
private void HandleOrderStart()
|
|
{
|
|
// Console.WriteLine("HandleOrderStart called");
|
|
OrderTimer = new(CycleHandlerMilliseconds, OrderHandler, Logger);
|
|
OrderTimer.Start();
|
|
}
|
|
|
|
private void HandleOrderStop()
|
|
{
|
|
OrderTimer?.Dispose();
|
|
OrderTimer = null;
|
|
OrderActions.Clear();
|
|
ActionWaitingRunning.Clear();
|
|
ActionManager.StopOrderAction(); // Stop all running order actions
|
|
// Reset state flags
|
|
IsCancelOrder = false;
|
|
IsCancelSentToNavigation = false;
|
|
IsNavigationFinished = false;
|
|
IsActionRunning = false;
|
|
IsWaitingPaused = false;
|
|
IsPaused = false;
|
|
ActionHard = null;
|
|
CurrentBaseNode = null;
|
|
Nodes = [];
|
|
Edges = [];
|
|
_currentActiveOrder = null;
|
|
// Reset EDGE action tracking
|
|
CurrentEdge = null;
|
|
RunningEdgeActionIds.Clear();
|
|
// Reset speed tracking
|
|
SafetySpeed = 0.0;
|
|
EdgeSpeed = 0.0;
|
|
CurrentSpeed = 0.0;
|
|
CachedNavConfig = null;
|
|
UpdateState();
|
|
SafetyManager.OnSafetySpeedChanged -= OnSafetySpeedChanged;
|
|
NavigationManager.OnNavigationFinished -= NavigationFinished;
|
|
StateManager.Fire(RobotEventType.CompleteExecution);
|
|
}
|
|
|
|
/// <summary>
|
|
/// VDA5050 Compliance: Normalize angle to range [-π, π]
|
|
/// </summary>
|
|
private static double NormalizeAngle(double angle)
|
|
{
|
|
while (angle > Math.PI) angle -= 2 * Math.PI;
|
|
while (angle < -Math.PI) angle += 2 * Math.PI;
|
|
return angle;
|
|
}
|
|
|
|
private Node? GetCurrentNode()
|
|
{
|
|
Node? inNode = null;
|
|
double minDistance = double.MaxValue;
|
|
foreach (var node in Nodes)
|
|
{
|
|
var distance = Localization.DistanceTo(node.NodePosition?.X ?? 0, node.NodePosition?.Y ?? 0);
|
|
var nodeMin = node.NodePosition?.AllowedDeviationXY == 0.0 ? 0.5 : node.NodePosition?.AllowedDeviationXY ?? 0.3;
|
|
|
|
bool positionMatch = distance <= nodeMin;
|
|
bool orientationMatch = true;
|
|
|
|
// VDA5050 Compliance: Check theta if specified
|
|
if (node.NodePosition?.Theta is not null)
|
|
{
|
|
var currentTheta = Localization.Theta;
|
|
var targetTheta = node.NodePosition.Theta.Value;
|
|
var allowedThetaDev = node.NodePosition.AllowedDeviationTheta ?? Math.PI; // Default: any orientation
|
|
|
|
var thetaDiff = Math.Abs(NormalizeAngle(currentTheta - targetTheta));
|
|
orientationMatch = thetaDiff <= allowedThetaDev;
|
|
}
|
|
|
|
if (positionMatch && orientationMatch)
|
|
{
|
|
// Exclude last node - it's handled separately in HandleOrder() after navigation completes (lines 392-399)
|
|
// This ensures intermediate node actions are processed during navigation
|
|
if (distance < minDistance && node.NodeId != Nodes[^1].NodeId)
|
|
{
|
|
minDistance = distance;
|
|
inNode = node;
|
|
}
|
|
}
|
|
}
|
|
return inNode;
|
|
}
|
|
|
|
private void NavigationFinished(NavigationState state)
|
|
{
|
|
NavState = state;
|
|
IsNavigationFinished = true;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Xử lý sự kiện thay đổi safety speed từ PLC
|
|
/// </summary>
|
|
private void OnSafetySpeedChanged(SafetySpeed safetySpeed)
|
|
{
|
|
if (TryGetSafetySpeedFromPlcSignal(safetySpeed, out double safeSpeed))
|
|
{
|
|
if (safetySpeed == Interfaces.SafetySpeed.Very_Fast)
|
|
{
|
|
SafetySpeed = 0.0; // 0 = không giới hạn từ safety; edge/config quyết định trong UpdateNavigationSpeed
|
|
UpdateNavigationSpeed();
|
|
Logger.LogInformation("SafetySpeed released: robot speed now follows edge/config limit");
|
|
}
|
|
else
|
|
{
|
|
SafetySpeed = safeSpeed;
|
|
UpdateNavigationSpeed();
|
|
}
|
|
}
|
|
else
|
|
{
|
|
Logger.LogWarning("Cannot map PLC SafetySpeed {SafetySpeed} to navigation speed value", safetySpeed);
|
|
}
|
|
}
|
|
|
|
private bool TryGetSafetySpeedFromPlcSignal(SafetySpeed safetySpeed, out double speed)
|
|
{
|
|
// PLC IO tốc độ: 825/2873, 826/2874, 827/2875, 828/2876.
|
|
// Ưu tiên map cứng theo yêu cầu vận hành để không phụ thuộc config.
|
|
switch (safetySpeed)
|
|
{
|
|
case Interfaces.SafetySpeed.Very_Slow:
|
|
speed = 1.0;
|
|
return true;
|
|
case Interfaces.SafetySpeed.Slow:
|
|
speed = 0.3;
|
|
return true;
|
|
case Interfaces.SafetySpeed.Normal:
|
|
speed = 0.6;
|
|
return true;
|
|
case Interfaces.SafetySpeed.Medium:
|
|
speed = 0.9;
|
|
return true;
|
|
case Interfaces.SafetySpeed.Very_Fast:
|
|
speed = 1.5;
|
|
return true;
|
|
}
|
|
|
|
// Các mức còn lại vẫn theo cấu hình hiện tại.
|
|
CachedNavConfig ??= NavigationConfig.GetNavigationConfig();
|
|
if (CachedNavConfig.SafetySpeedMap.TryGetValue(safetySpeed, out double configSpeed))
|
|
{
|
|
speed = configSpeed;
|
|
return true;
|
|
}
|
|
|
|
speed = 0.0;
|
|
return false;
|
|
}
|
|
|
|
/// <summary>
|
|
/// VDA5050 Compliance: Tính toán và áp dụng tốc độ giới hạn cho navigation
|
|
/// Kết hợp 3 nguồn tốc độ: Edge, Safety, Config Max
|
|
/// Sử dụng giá trị MIN để đảm bảo an toàn
|
|
/// </summary>
|
|
private void UpdateNavigationSpeed()
|
|
{
|
|
if (Nodes.Length <= 0 || IsNavigationFinished)
|
|
return;
|
|
|
|
CachedNavConfig ??= NavigationConfig.GetNavigationConfig();
|
|
double maxConfigSpeed = CachedNavConfig.MaxLinearVelocity;
|
|
if (maxConfigSpeed <= 0)
|
|
return;
|
|
|
|
// Bắt đầu từ max config; EdgeSpeed/SafetySpeed = 0 nghĩa là không giới hạn từ nguồn đó
|
|
double targetSpeed = maxConfigSpeed;
|
|
|
|
if (EdgeSpeed > 0)
|
|
targetSpeed = Math.Min(targetSpeed, EdgeSpeed);
|
|
|
|
if (SafetySpeed > 0)
|
|
targetSpeed = Math.Min(targetSpeed, SafetySpeed);
|
|
|
|
targetSpeed = Math.Max(targetSpeed, CachedNavConfig.MinLinearVelocity);
|
|
|
|
if (Math.Abs(CurrentSpeed - targetSpeed) < 0.01)
|
|
return;
|
|
|
|
CurrentSpeed = targetSpeed;
|
|
NavigationManager.SetSpeed(CurrentSpeed);
|
|
|
|
Logger.LogInformation(
|
|
"Speed updated: {CurrentSpeed:F2} m/s [Edge: {EdgeSpeed:F2}, Safety: {SafetySpeed:F2}, Max: {MaxSpeed:F2}]",
|
|
CurrentSpeed,
|
|
EdgeSpeed > 0 ? EdgeSpeed : maxConfigSpeed,
|
|
SafetySpeed > 0 ? SafetySpeed : maxConfigSpeed,
|
|
maxConfigSpeed);
|
|
}
|
|
|
|
private void UpdateState()
|
|
{
|
|
NodeStates = [.. Nodes.Select(n => new NodeState
|
|
{
|
|
NodeId = n.NodeId,
|
|
Released = n.Released,
|
|
SequenceId = n.SequenceId,
|
|
NodeDescription = n.NodeDescription,
|
|
NodePosition = n.NodePosition is null ? null : 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 async Task ClearOldOrder()
|
|
{
|
|
OrderActions.Clear();
|
|
await ActionManager.ClearActions();
|
|
IsNavigationFinished = false;
|
|
IsCancelOrder = false;
|
|
IsActionRunning = false;
|
|
IsWaitingPaused = false;
|
|
ActionHard = null;
|
|
}
|
|
|
|
private void AddAction(Action[] actions, Node node)
|
|
{
|
|
foreach (var item in actions)
|
|
{
|
|
item.ActionDescription += $".On Node: {(string.IsNullOrEmpty(node.NodeDescription) ? node.NodeId : node.NodeDescription)}";
|
|
}
|
|
if (OrderActions.TryGetValue(node.NodeId, out Action[]? oldActions) && oldActions is not null)
|
|
{
|
|
OrderActions[node.NodeId] = [.. oldActions, .. actions];
|
|
}
|
|
else OrderActions.Add(node.NodeId, actions);
|
|
}
|
|
|
|
private void AddEdgeAction(Action[] actions, Edge edge)
|
|
{
|
|
foreach (var item in actions)
|
|
{
|
|
item.ActionDescription += $".On Edge: {(string.IsNullOrEmpty(edge.EdgeDescription) ? edge.EdgeId : edge.EdgeDescription)}";
|
|
}
|
|
if (OrderActions.TryGetValue(edge.EdgeId, out Action[]? oldActions) && oldActions is not null)
|
|
{
|
|
OrderActions[edge.EdgeId] = [.. oldActions, .. actions];
|
|
}
|
|
else OrderActions.Add(edge.EdgeId, actions);
|
|
}
|
|
|
|
private void ValidateNodes(Node[] nodes, int currentSequence)
|
|
{
|
|
for (int i = 0; i < nodes.Length; i++)
|
|
{
|
|
int correctSequence = i * 2 + currentSequence;
|
|
if (nodes[i].SequenceId != correctSequence) throw new OrderException(RobotErrors.Error1012(nodes[i].NodeId, nodes[i].SequenceId, correctSequence));
|
|
if (nodes[i].NodePosition is null) throw new OrderException(RobotErrors.Error1015(nodes[i].NodeId));
|
|
if (i == 0)
|
|
{
|
|
if (nodes[i].Released)
|
|
{
|
|
if (nodes[i].Actions != null && nodes[i].Actions.Length > 0) AddAction(nodes[i].Actions, nodes[i]);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
private static void ValidateTrajectory(Edge edge, Node startNode, Node endNode)
|
|
{
|
|
// VDA5050 Compliance: Validate NURBS trajectory structure
|
|
if (edge.Trajectory is not null)
|
|
{
|
|
var traj = edge.Trajectory;
|
|
|
|
// Validate controlPoints count (minimum 2: start and end)
|
|
if (traj.ControlPoints is null || traj.ControlPoints.Length < 2)
|
|
{
|
|
throw new OrderException(RobotErrors.Error1020(edge.EdgeId));
|
|
}
|
|
|
|
// Validate knotVector size: must equal controlPoints.Length + degree + 1
|
|
if (traj.KnotVector is not null)
|
|
{
|
|
int expectedSize = traj.ControlPoints.Length + traj.Degree + 1;
|
|
if (traj.KnotVector.Length != expectedSize)
|
|
{
|
|
throw new OrderException(RobotErrors.Error1018(edge.EdgeId));
|
|
}
|
|
|
|
// Validate knotVector is monotonically increasing from 0 to 1
|
|
for (int j = 0; j < traj.KnotVector.Length; j++)
|
|
{
|
|
if (traj.KnotVector[j] < 0 || traj.KnotVector[j] > 1)
|
|
{
|
|
throw new OrderException(RobotErrors.Error1019(edge.EdgeId));
|
|
}
|
|
if (j > 0 && traj.KnotVector[j] < traj.KnotVector[j - 1])
|
|
{
|
|
throw new OrderException(RobotErrors.Error1019(edge.EdgeId));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// VDA5050 Compliance: Create valid default linear trajectory
|
|
edge.Trajectory = new Trajectory()
|
|
{
|
|
Degree = 1,
|
|
ControlPoints =
|
|
[
|
|
new ControlPoint()
|
|
{
|
|
X = startNode.NodePosition?.X ?? 0,
|
|
Y = startNode.NodePosition?.Y ?? 0,
|
|
Weight = 1.0
|
|
},
|
|
new ControlPoint()
|
|
{
|
|
X = endNode.NodePosition?.X ?? 0,
|
|
Y = endNode.NodePosition?.Y ?? 0,
|
|
Weight = 1.0
|
|
}
|
|
],
|
|
KnotVector = [0, 0, 1, 1]
|
|
};
|
|
}
|
|
}
|
|
|
|
private void ValidateEdges(Edge[] edges, Node[] nodes, int currentSequence)
|
|
{
|
|
for (int i = 0; i < edges.Length; i++)
|
|
{
|
|
var startNode = nodes.FirstOrDefault(n => n.NodeId == edges[i].StartNodeId) ??
|
|
throw new OrderException(RobotErrors.Error1008(edges[i].EdgeId, edges[i].StartNodeId));
|
|
var endNode = nodes.FirstOrDefault(n => n.NodeId == edges[i].EndNodeId) ??
|
|
throw new OrderException(RobotErrors.Error1009(edges[i].EdgeId, edges[i].StartNodeId));
|
|
|
|
int correctSequence = i * 2 + 1 + currentSequence;
|
|
if (edges[i].SequenceId != correctSequence) throw new OrderException(RobotErrors.Error1013(edges[i].EdgeId, edges[i].SequenceId, correctSequence));
|
|
|
|
// VDA5050 Compliance: Validate or create proper trajectory
|
|
ValidateTrajectory(edges[i], startNode, endNode);
|
|
|
|
if (edges[i].Released)
|
|
{
|
|
if (endNode.Released)
|
|
{
|
|
CurrentBaseNode = endNode;
|
|
if (endNode.Actions != null && endNode.Actions.Length > 0) AddAction(endNode.Actions, endNode);
|
|
if (edges[i].Actions != null && edges[i].Actions.Length > 0) AddEdgeAction(edges[i].Actions, edges[i]);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
private async Task HandleNewOrder(OrderMsg order)
|
|
{
|
|
if (order.OrderId == OrderId)
|
|
{
|
|
if (order.OrderUpdateId < OrderUpdateId) throw new OrderException(RobotErrors.Error1003(OrderUpdateId, order.OrderUpdateId));
|
|
if (order.OrderUpdateId == OrderUpdateId) return;
|
|
if (order.Nodes[0].NodeId != LastNodeId)
|
|
{
|
|
throw new OrderException(RobotErrors.Error1010(LastNodeId, order.Nodes[0].NodeId));
|
|
}
|
|
if (order.Nodes[0].SequenceId != LastNodeSequenceId)
|
|
{
|
|
throw new OrderException(RobotErrors.Error1011(LastNodeSequenceId, order.Nodes[0].SequenceId));
|
|
}
|
|
}
|
|
|
|
// xử lí order mới
|
|
// Validate Nodes, Edges
|
|
await ClearOldOrder();
|
|
ValidateNodes(order.Nodes, order.OrderId == OrderId ? LastNodeSequenceId : 0);
|
|
ValidateEdges(order.Edges, order.Nodes, order.OrderId == OrderId ? LastNodeSequenceId : 0);
|
|
|
|
// Add actions to ActionManager with correct scope
|
|
if (OrderActions.Count > 0)
|
|
{
|
|
foreach (var actions in OrderActions)
|
|
{
|
|
ActionManager.AddOrderActions(actions.Value, order.Edges.Any(e => e.EdgeId == actions.Key) ? ActionScope.EDGE : ActionScope.NODE);
|
|
}
|
|
}
|
|
|
|
if (order.Nodes.Length <= 1 || order.Edges.Length == 0)
|
|
{
|
|
if (order.Nodes.Length == 1 && order.Nodes[0].Actions.Length == 0) return;
|
|
NavigationFinished(NavigationState.Completed);
|
|
}
|
|
|
|
OrderId = order.OrderId;
|
|
OrderUpdateId = order.OrderUpdateId;
|
|
Nodes = order.Nodes;
|
|
Edges = order.Edges;
|
|
_currentActiveOrder = order;
|
|
ErrorManager.DeleteErrorType(ErrorType.VALIDATION_ERROR.ToString());
|
|
ErrorManager.DeleteErrorType(ErrorType.ORDER_ERROR.ToString());
|
|
ErrorManager.DeleteErrorType(ErrorType.ORDER_UPDATE_ERROR.ToString());
|
|
UpdateState();
|
|
HasNewOrder = true;
|
|
}
|
|
|
|
private void ClearLastNode()
|
|
{
|
|
if (LastNode is null) return;
|
|
var currentLastNodeIndex = Array.FindIndex(Nodes, n => n.NodeId == LastNode.NodeId);
|
|
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));
|
|
if (order.OrderUpdateId < OrderUpdateId) throw new OrderException(RobotErrors.Error1003(OrderUpdateId, order.OrderUpdateId));
|
|
if (order.OrderUpdateId == OrderUpdateId) return;
|
|
|
|
if (CurrentBaseNode is not null && order.Nodes[0].NodeId != CurrentBaseNode.NodeId)
|
|
{
|
|
throw new OrderException(RobotErrors.Error1010(LastNodeId, order.Nodes[0].NodeId));
|
|
}
|
|
if (CurrentBaseNode is not null && order.Nodes[0].SequenceId != CurrentBaseNode.SequenceId)
|
|
{
|
|
throw new OrderException(RobotErrors.Error1011(LastNodeSequenceId, order.Nodes[0].SequenceId));
|
|
}
|
|
|
|
IsNavigationFinished = false;
|
|
|
|
Node[] baseNodes = CurrentBaseNode is null ? [] : [.. Nodes.TakeWhile(n => n != CurrentBaseNode).Append(CurrentBaseNode)];
|
|
Edge[] baseEdges = CurrentBaseNode is null ? [] : [.. Edges.ToList().GetRange(0, baseNodes.Length - 1)];
|
|
|
|
ValidateNodes(order.Nodes, baseNodes.Length > 0 ? baseNodes[^1].SequenceId : 0);
|
|
ValidateEdges(order.Edges, order.Nodes, baseNodes.Length > 0 ? baseNodes[^1].SequenceId : 0);
|
|
|
|
if (OrderActions.Count > 0)
|
|
{
|
|
foreach(var actions in OrderActions)
|
|
{
|
|
ActionManager.AddOrderActions(actions.Value, order.Edges.Any(e => e.EdgeId == actions.Key) ? ActionScope.EDGE : ActionScope.NODE);
|
|
}
|
|
}
|
|
|
|
OrderUpdateId = order.OrderUpdateId;
|
|
Nodes = [.. baseNodes, .. order.Nodes.Skip(1)];
|
|
Edges = [.. baseEdges, .. order.Edges];
|
|
_currentActiveOrder = new OrderMsg
|
|
{
|
|
HeaderId = order.HeaderId,
|
|
Timestamp = order.Timestamp,
|
|
Version = order.Version,
|
|
Manufacturer = order.Manufacturer,
|
|
SerialNumber = order.SerialNumber,
|
|
OrderId = order.OrderId,
|
|
OrderUpdateId = order.OrderUpdateId,
|
|
ZoneSetId = order.ZoneSetId,
|
|
Nodes = Nodes,
|
|
Edges = Edges
|
|
};
|
|
|
|
ErrorManager.DeleteErrorType(ErrorType.VALIDATION_ERROR.ToString());
|
|
ErrorManager.DeleteErrorType(ErrorType.ORDER_ERROR.ToString());
|
|
ErrorManager.DeleteErrorType(ErrorType.ORDER_UPDATE_ERROR.ToString());
|
|
UpdateState();
|
|
}
|
|
|
|
private void StartActionTerminal(Node node)
|
|
{
|
|
var action = node.Actions[0];
|
|
var robotAction = ActionManager[action.ActionId];
|
|
if (robotAction is null)
|
|
{
|
|
if (!ActionManager.HasActionWaitting && node.Actions.Length > 0) node.Actions = [.. node.Actions.Skip(1)];
|
|
return;
|
|
}
|
|
if (robotAction.IsCompleted) node.Actions = [.. node.Actions.Skip(1)];
|
|
if (robotAction.Status == ActionStatus.WAITING) ActionManager.StartOrderAction(action.ActionId);
|
|
}
|
|
|
|
private void HandleOrder()
|
|
{
|
|
if (Nodes.Length <= 0)
|
|
{
|
|
HandleOrderStop();
|
|
return;
|
|
}
|
|
|
|
if (HasNewOrder)
|
|
{
|
|
if (ActionManager.HasActionWaitting) return;
|
|
if (Nodes.Length > 1 && Edges.Length >= 0)
|
|
{
|
|
if (Nodes[0].Actions.Length > 0)
|
|
{
|
|
// VDA5050: Check if robot is on Node[0] before triggering actions
|
|
var startNode = Nodes[0];
|
|
var nodeDeviation = startNode.NodePosition?.AllowedDeviationXY ?? 0.5;
|
|
if (nodeDeviation == 0.0) nodeDeviation = 0.5;
|
|
var distance = Localization.DistanceTo(startNode.NodePosition?.X ?? 0, startNode.NodePosition?.Y ?? 0);
|
|
|
|
if (distance <= nodeDeviation)
|
|
{
|
|
// VDA5050: Separate NONE from blocking actions on Node[0]
|
|
var noneActions = startNode.Actions.Where(a => a.BlockingType == BlockingType.NONE).ToArray();
|
|
var blockingActions = startNode.Actions.Where(a => a.BlockingType != BlockingType.NONE).ToArray();
|
|
|
|
// Start NONE actions immediately - they must not delay navigation
|
|
foreach (var action in noneActions)
|
|
{
|
|
ActionManager.StartOrderAction(action.ActionId);
|
|
}
|
|
|
|
startNode.Actions = blockingActions;
|
|
|
|
if (blockingActions.Length > 0)
|
|
{
|
|
// Robot is on Node[0] - trigger blocking actions sequentially
|
|
StartActionTerminal(Nodes[0]);
|
|
return;
|
|
}
|
|
// All were NONE → fall through to start navigation
|
|
}
|
|
// else: Robot not on Node[0] - let navigation start, actions will trigger when node is traversed
|
|
}
|
|
else
|
|
// Start navigation (no blocking actions on Node[0], or Node[0] has no actions)
|
|
{
|
|
IsCancelSentToNavigation = false;
|
|
NavigationManager.OnNavigationFinished += NavigationFinished;
|
|
SafetyManager.OnSafetySpeedChanged += OnSafetySpeedChanged;
|
|
|
|
// Cache NavigationConfig để tránh load lại mỗi lần speed change
|
|
CachedNavConfig = NavigationConfig.GetNavigationConfig();
|
|
|
|
// VDA5050: Đọc initial safety speed khi bắt đầu navigation
|
|
var currentSafetySpeed = SafetyManager.SafetySpeed;
|
|
if (TryGetSafetySpeedFromPlcSignal(currentSafetySpeed, out double safeSpeed))
|
|
{
|
|
SafetySpeed = safeSpeed;
|
|
Logger.LogInformation("Initial safety speed: {SafetySpeed:F2} m/s (level: {Level})", SafetySpeed, currentSafetySpeed);
|
|
}
|
|
|
|
// VDA5050: Set initial edge speed (edge đầu tiên)
|
|
if (Edges.Length > 0 && Edges[0].MaxSpeed.HasValue && Edges[0].MaxSpeed is double speed)
|
|
{
|
|
EdgeSpeed = speed;
|
|
Logger.LogInformation("Initial edge speed: {EdgeSpeed:F2} m/s (edge: {EdgeId})", EdgeSpeed, Edges[0].EdgeId);
|
|
}
|
|
else
|
|
{
|
|
EdgeSpeed = 0.0; // Không giới hạn
|
|
}
|
|
|
|
// VDA5050: Áp dụng tốc độ ban đầu trước khi bắt đầu navigation
|
|
UpdateNavigationSpeed();
|
|
|
|
// chỗ này có thể sẽ phải sửa lại theo interface của a Hiệp
|
|
NavigationManager.Move(_currentActiveOrder!, SafetyManager.SetHasLoadValue);
|
|
if (CurrentBaseNode is not null
|
|
&& CurrentBaseNode.NodeId != Nodes[0].NodeId
|
|
&& CurrentBaseNode.NodeId != Nodes[^1].NodeId
|
|
&& Nodes.Length > 1)
|
|
{
|
|
NavigationManager.UpdateOrder(CurrentBaseNode.NodeId);
|
|
}
|
|
if (StateManager.CurrentState != RobotStateType.Executing) StateManager.Fire(RobotEventType.StartExecution);
|
|
if(OrderActions.ContainsKey(Nodes[0].NodeId)) OrderActions.Remove(Nodes[0].NodeId);
|
|
HasNewOrder = false;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (IsCancelOrder && !IsCancelSentToNavigation)
|
|
{
|
|
NavigationManager.CancelMovement();
|
|
IsCancelSentToNavigation = true;
|
|
}
|
|
|
|
if (IsNavigationFinished)
|
|
{
|
|
if (IsCancelOrder && !ActionManager.HasActionRunning)
|
|
{
|
|
HandleOrderStop();
|
|
Logger.LogInformation("Order {OrderId} is canceled", OrderId);
|
|
}
|
|
else if (NavState == NavigationState.Completed)
|
|
{
|
|
if (Nodes.Length > 0 && Nodes[^1].Actions.Length > 0) StartActionTerminal(Nodes[^1]);
|
|
else if (ActionManager.HasActionRunning) return;
|
|
else
|
|
{
|
|
LastNode = Nodes[^1];
|
|
HandleOrderStop();
|
|
Logger.LogInformation("Order {OrderId} is finished", OrderId);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (NavState == NavigationState.Error) ErrorManager.AddError(RobotErrors.Error1014());
|
|
HandleOrderStop();
|
|
Logger.LogInformation("Order {OrderId} is error", OrderId);
|
|
}
|
|
return;
|
|
}
|
|
|
|
var currentNode = GetCurrentNode();
|
|
if (currentNode is not null && currentNode.NodeId != LastNode?.NodeId)
|
|
{
|
|
LastNode = currentNode;
|
|
|
|
// VDA5050 Section 6.10.2: Finish EDGE actions from previous edge when leaving it
|
|
if (CurrentEdge is not null && !RunningEdgeActionIds.IsEmpty)
|
|
{
|
|
Logger.LogInformation("Finishing {Count} EDGE actions from edge {EdgeId}", RunningEdgeActionIds.Count, CurrentEdge.EdgeId);
|
|
foreach (var actionId in RunningEdgeActionIds.ToList())
|
|
{
|
|
ActionManager.FinishAction(actionId);
|
|
}
|
|
RunningEdgeActionIds.Clear();
|
|
}
|
|
|
|
// VDA5050: Cập nhật edge speed và start EDGE actions khi robot vào edge tiếp theo
|
|
// Khi đến node i, robot sẽ bắt đầu đi trên edge i (từ node i → node i+1)
|
|
var currentNodeIndex = Array.FindIndex(Nodes, n => n.NodeId == currentNode.NodeId);
|
|
if (currentNodeIndex >= 0 && currentNodeIndex < Edges.Length)
|
|
{
|
|
var nextEdge = Edges[currentNodeIndex];
|
|
CurrentEdge = nextEdge;
|
|
|
|
// Update edge speed
|
|
if (nextEdge.MaxSpeed.HasValue && nextEdge.MaxSpeed.Value > 0)
|
|
{
|
|
EdgeSpeed = nextEdge.MaxSpeed.Value;
|
|
Logger.LogInformation("Edge speed updated: {EdgeSpeed:F2} m/s (edge: {EdgeId}, node: {NodeId})",
|
|
EdgeSpeed, nextEdge.EdgeId, currentNode.NodeId);
|
|
}
|
|
else
|
|
{
|
|
EdgeSpeed = 0.0; // Không giới hạn
|
|
Logger.LogInformation("Edge speed limit removed (edge: {EdgeId}, node: {NodeId})",
|
|
nextEdge.EdgeId, currentNode.NodeId);
|
|
}
|
|
|
|
UpdateNavigationSpeed();
|
|
|
|
// VDA5050: Start EDGE actions for this edge
|
|
// (Actions already added to ActionManager during HandleNewOrder)
|
|
if (nextEdge.Actions.Length > 0)
|
|
{
|
|
Logger.LogInformation("Starting {Count} EDGE actions for edge {EdgeId}", nextEdge.Actions.Length, nextEdge.EdgeId);
|
|
|
|
// Separate NONE from blocking EDGE actions
|
|
var noneActions = nextEdge.Actions.Where(a => a.BlockingType == BlockingType.NONE).ToArray();
|
|
var blockingActions = nextEdge.Actions.Where(a => a.BlockingType == BlockingType.SOFT || a.BlockingType == BlockingType.HARD).ToArray();
|
|
|
|
// Start NONE actions immediately
|
|
foreach (var action in noneActions)
|
|
{
|
|
ActionManager.StartOrderAction(action.ActionId);
|
|
RunningEdgeActionIds.Add(action.ActionId);
|
|
}
|
|
|
|
// Pause navigation and enqueue blocking actions
|
|
if (blockingActions.Length > 0)
|
|
{
|
|
NavigationManager.Pause();
|
|
IsWaitingPaused = true;
|
|
|
|
foreach (var action in blockingActions)
|
|
{
|
|
ActionWaitingRunning.Enqueue(action);
|
|
RunningEdgeActionIds.Add(action.ActionId);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// No next edge - clear current edge
|
|
CurrentEdge = null;
|
|
}
|
|
|
|
if (OrderActions.TryGetValue(currentNode.NodeId, out Action[]? actions) && actions is not null && actions.Length > 0)
|
|
{
|
|
// VDA5050 Compliance: Separate NONE actions from blocking actions
|
|
var noneActions = actions.Where(a => a.BlockingType == BlockingType.NONE).ToArray();
|
|
var blockingActions = actions.Where(a => a.BlockingType == BlockingType.SOFT || a.BlockingType == BlockingType.HARD).ToArray();
|
|
|
|
// Start NONE actions immediately - they can run during movement
|
|
foreach (var action in noneActions)
|
|
{
|
|
ActionManager.StartOrderAction(action.ActionId);
|
|
}
|
|
|
|
// Pause navigation only if there are SOFT/HARD actions
|
|
if (blockingActions.Length > 0)
|
|
{
|
|
NavigationManager.Pause();
|
|
IsWaitingPaused = true;
|
|
|
|
// Enqueue blocking actions for sequential execution
|
|
foreach (var action in blockingActions)
|
|
{
|
|
ActionWaitingRunning.Enqueue(action);
|
|
}
|
|
}
|
|
}
|
|
|
|
ClearLastNode();
|
|
}
|
|
|
|
UpdateNavigationSpeed();
|
|
|
|
// VDA5050: Improved blocking logic for parallel SOFT actions
|
|
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)
|
|
{
|
|
// VDA5050: Check if there are running SOFT actions (both NODE and EDGE)
|
|
var runningSoftActions = ActionManager.GetRunningActions()
|
|
.Where(a => (a.ActionScope == ActionScope.NODE || a.ActionScope == ActionScope.EDGE) &&
|
|
a.BlockingType == BlockingType.SOFT)
|
|
.ToList();
|
|
|
|
// Try to start next action(s) from queue
|
|
while (!ActionWaitingRunning.IsEmpty)
|
|
{
|
|
if (ActionWaitingRunning.TryPeek(out Action? action) && action is not null)
|
|
{
|
|
var robotAction = ActionManager[action.ActionId];
|
|
if (robotAction is null)
|
|
{
|
|
// Action not found - dequeue and skip it
|
|
ActionWaitingRunning.TryDequeue(out _);
|
|
Logger.LogWarning($"Action {action.ActionId} (type: {action.ActionType}) not found in ActionManager - skipping action");
|
|
continue;
|
|
}
|
|
|
|
// VDA5050: Check if action can start based on blocking type
|
|
if (action.BlockingType == BlockingType.HARD)
|
|
{
|
|
// HARD can only start if no actions are running
|
|
if (runningSoftActions.Count > 0)
|
|
{
|
|
// Wait for SOFT actions to complete
|
|
break;
|
|
}
|
|
// Start HARD action and set flag
|
|
ActionWaitingRunning.TryDequeue(out _);
|
|
ActionManager.StartOrderAction(action.ActionId);
|
|
ActionHard = action;
|
|
break; // Only one HARD action at a time
|
|
}
|
|
else if (action.BlockingType == BlockingType.SOFT)
|
|
{
|
|
// SOFT can start in parallel with other SOFT actions
|
|
ActionWaitingRunning.TryDequeue(out _);
|
|
ActionManager.StartOrderAction(action.ActionId);
|
|
runningSoftActions.Add(robotAction);
|
|
// Continue to potentially start more SOFT actions
|
|
}
|
|
else
|
|
{
|
|
// NONE should have been started already, but handle it anyway
|
|
ActionWaitingRunning.TryDequeue(out _);
|
|
ActionManager.StartOrderAction(action.ActionId);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (IsWaitingPaused)
|
|
{
|
|
IsWaitingPaused = false;
|
|
NavigationManager.Resume();
|
|
if (CurrentBaseNode is not null
|
|
&& CurrentBaseNode.NodeId != Nodes[0].NodeId
|
|
&& CurrentBaseNode.NodeId != Nodes[^1].NodeId
|
|
&& Nodes.Length > 1)
|
|
{
|
|
NavigationManager.UpdateOrder(CurrentBaseNode.NodeId);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
private async void OrderHandler()
|
|
{
|
|
try
|
|
{
|
|
if (NewOrder is not null)
|
|
{
|
|
OrderMsg NewOrderHandler;
|
|
lock (LockObject)
|
|
{
|
|
NewOrderHandler = NewOrder;
|
|
NewOrder = null;
|
|
}
|
|
|
|
if (NewOrderHandler.Nodes.Length == 0) 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
|
|
{
|
|
if (ActionManager.HasActionRunning) return;
|
|
// Kiểm tra robot có nằm trên node đầu tien không
|
|
Node startNode = NewOrderHandler.Nodes[0];
|
|
var nodeDeviation = startNode.NodePosition?.AllowedDeviationXY == 0.0 ? NewOrderHandler.Nodes.Length == 1 ? 0.3 : 0.5 : startNode.NodePosition?.AllowedDeviationXY ?? 0.5;
|
|
var distance = Localization.DistanceTo(startNode.NodePosition?.X ?? 0, startNode.NodePosition?.Y ?? 0);
|
|
if (distance > nodeDeviation) throw new OrderException(RobotErrors.Error1016(startNode.NodeId, distance, nodeDeviation));
|
|
|
|
if (NewOrderHandler.Nodes.Length > 1)
|
|
{
|
|
Node endNode = NewOrderHandler.Nodes[^1];
|
|
nodeDeviation = endNode.NodePosition?.AllowedDeviationXY == 0.0 ? 0.2 : endNode.NodePosition?.AllowedDeviationXY ?? 0.2;
|
|
distance = Localization.DistanceTo(endNode.NodePosition?.X ?? 0, endNode.NodePosition?.Y ?? 0);
|
|
if (distance < nodeDeviation) throw new OrderException(RobotErrors.Error1017(endNode.NodeId, distance, nodeDeviation));
|
|
}
|
|
await HandleNewOrder(NewOrderHandler);
|
|
}
|
|
}
|
|
HandleOrder();
|
|
}
|
|
catch (RobotException orEx)
|
|
{
|
|
if (orEx.Error is not null)
|
|
{
|
|
ErrorManager.AddError(orEx.Error);
|
|
Logger.LogWarning("Order processing error: {orEx.Error.ErrorDescription}", orEx.Error.ErrorDescription);
|
|
}
|
|
else Logger.LogWarning("Order processing error: {orEx.Message}", orEx.Message);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
Logger.LogWarning("Order processing error: {ex.Message}", ex.Message);
|
|
}
|
|
}
|
|
|
|
}
|