using RobotNet.MapShares.Dtos; using RobotNet.RobotManager.Services.OpenACS; using RobotNet.RobotManager.Services.Planner.Space; using RobotNet.RobotManager.Services.Traffic; using RobotNet.RobotShares.Dtos; using RobotNet.RobotShares.Enums; using RobotNet.RobotShares.VDA5050.Order; using RobotNet.RobotShares.VDA5050.State; using RobotNet.Shares; using SixLabors.ImageSharp; using Action = RobotNet.RobotShares.VDA5050.InstantAction.Action; namespace RobotNet.RobotManager.Services.Robot; public class RobotOrder : IRobotOrder, IDisposable { public bool IsError { get; private set; } public bool IsCompleted { get; private set; } public bool IsProcessing => !IsDisposed; public bool IsCanceled { get; private set; } public string[] Errors => [.. GetError()]; public Guid MapId { get; private set; } public TrafficSolutionState TrafficSolutionState { get; private set; } = TrafficSolutionState.None; public NavigationPathEdge[] FullPath => GetFullPath(); public NavigationPathEdge[] BasePath => GetBasePath(); public List CurrentZones { get; set; } = []; private const int intervalTime = 50; private readonly WatchTimerAsync Timer; private int TimerCounter = 0; private OrderMsg OrderMsg; private readonly LoggerController Logger; private readonly TrafficManager TrafficManager; private readonly TrafficACS TrafficACS; private readonly MapManager MapManager; private readonly Func> PubOrder; private readonly List Nodes; private readonly List Edges; private readonly Dictionary Zones; private readonly IDictionary> Actions; private readonly IRobotController RobotController; private readonly NavigationType NavigationType; private Guid TrafficManagerGoalId = Guid.Empty; private Guid TrafficACSGoalId = Guid.Empty; private bool IsDisposed = false; private bool IsWaittingCancel = false; private Guid CurrentBaseId = Guid.Empty; private Guid LastNodeId = Guid.Empty; public RobotOrder(NodeDto[] nodes, EdgeDto[] edges, Dictionary zones, IDictionary> actions, OrderMsg orderMsg, IRobotController robotController, NavigationType navigationType, TrafficManager traffiManager, TrafficACS trafficACS, MapManager mapManager, LoggerController logger, Func> pubOrder) { if (nodes.Length < 2) { IsError = true; IsDisposed = true; throw new ArgumentException("Đường dẫn không hợp lệ. Số lượng nodes nhỏ hơn 2"); } if (nodes.Length < 1) { IsError = true; IsDisposed = true; throw new ArgumentException("Đường dẫn không hợp lệ. Số lượng edges nhỏ hơn 1"); } Nodes = [.. nodes]; Edges = [.. edges]; Zones = zones ?? []; MapId = Nodes[1].MapId; Actions = actions; OrderMsg = orderMsg; RobotController = robotController; NavigationType = navigationType; TrafficManager = traffiManager; TrafficACS = trafficACS; MapManager = mapManager; Logger = logger; PubOrder = pubOrder; Timer = new(intervalTime, TimerHandler, logger); Timer.Start(); } private async Task PublishOrder(Guid goalId) { OrderMsg.OrderUpdateId++; OrderMsg.HeaderId++; OrderMsg.Timestamp = DateTime.Now.ToString("yyyy-MM-ddTHH:mm:ss.fffZ"); var pubOrder = await PubOrder.Invoke(OrderMsg); if (!pubOrder.IsSuccess) { OrderMsg.OrderUpdateId--; OrderMsg.HeaderId--; return false; } CurrentBaseId = goalId; return true; } private void UpdatePath(NodeDto[] nodes, EdgeDto[] edges) { try { var map = Task.Run(async () => await MapManager.GetMapData(MapId)); map.Wait(); if (map.Result is null) Logger.Warning($"{RobotController.StateMsg.SerialNumber} - Robot Order cập nhật tuyến đường mới có lỗi xảy ra: không thể lấy dữ liệu bản đồ {MapId}"); else if (!map.Result.IsSuccess) Logger.Warning($"{RobotController.StateMsg.SerialNumber} - Robot Order cập nhật tuyến đường mới có lỗi xảy ra: {map.Result.Message}"); else if (map.Result.Data is not null) { Robot.RobotController.CreateOrderMsg(ref OrderMsg, nodes, edges, map.Result.Data, Actions, NavigationType, TrafficManager.Enable); Nodes.Clear(); Nodes.AddRange(nodes); Edges.Clear(); Edges.AddRange(edges); } else Logger.Warning($"{RobotController.StateMsg.SerialNumber} - Robot Order cập nhật tuyến đường mới có lỗi xảy ra: dữ liệu bản đồ {MapId} rỗng"); } catch (Exception ex) { Logger.Warning($"{RobotController.StateMsg.SerialNumber} - Robot Order cập nhật tuyến đường mới có lỗi xảy ra: {ex}"); } } private bool UpdateGoal(Guid goalId) { var currentNodeOrderIndex = Array.FindIndex(OrderMsg.Nodes, n => n.NodeId == LastNodeId.ToString()); if (currentNodeOrderIndex != -1) { OrderMsg.Nodes = [.. OrderMsg.Nodes.Skip(currentNodeOrderIndex)]; OrderMsg.Edges = [.. OrderMsg.Edges.Skip(currentNodeOrderIndex)]; } var goalOrder = OrderMsg.Nodes.FirstOrDefault(n => n.NodeId == goalId.ToString()); if (goalOrder is not null && CurrentBaseId.ToString() != goalOrder.NodeId) { for (int i = 0; i <= Array.IndexOf(OrderMsg.Nodes, goalOrder); i++) { OrderMsg.Nodes[i].Released = true; } for (int i = 0; i < Array.IndexOf(OrderMsg.Nodes, goalOrder); i++) { OrderMsg.Edges[i].Released = true; } return true; } return false; } private void HandleGiveWayState(TrafficSolution solution) { if (solution.GivewayNodes[^1].Id != solution.ReleaseNode.Id) return; if (solution.GivewayEdges.Count == 0) Logger.Warning($"{RobotController.StateMsg.SerialNumber} - Robot Order xử lí tránh đường lỗi: Không có edge mới nhận được"); else if (solution.GivewayNodes.Count <= 1) Logger.Warning($"{RobotController.StateMsg.SerialNumber} - Robot Order xử lí tránh đường lỗi: nodes mới nhận được có số lượng {solution.Nodes.Length}"); else if (CurrentBaseId != solution.ReleaseNode.Id) { List nodes = [..solution.GivewayNodes.Select(n => new NodeDto() { Id = n.Id, Name = n.Name, X = n.X, Y = n.Y, Direction = MapCompute.GetNodeDirection(n.Direction), })]; List edges = [..solution.GivewayEdges.Select(e => new EdgeDto() { Id = e.Id, ControlPoint1X = e.ControlPoint1X, ControlPoint2X = e.ControlPoint2X, ControlPoint1Y = e.ControlPoint1Y, ControlPoint2Y = e.ControlPoint2Y, TrajectoryDegree = e.TrajectoryDegree, StartNodeId = e.StartNodeId, EndNodeId = e.EndNodeId, })]; nodes.Add(nodes[^2]); edges.Add(new() { Id = edges[^1].Id, StartNodeId = nodes[^2].Id, EndNodeId = nodes[^1].Id, ControlPoint1X = edges[^1].ControlPoint2X, ControlPoint1Y = edges[^1].ControlPoint2Y, ControlPoint2X = edges[^1].ControlPoint1X, ControlPoint2Y = edges[^1].ControlPoint1Y, TrajectoryDegree = edges[^1].TrajectoryDegree, }); UpdatePath([.. nodes], [.. edges]); } } private void HandleRefreshPath(TrafficSolution solution) { if (solution.Edges.Length == 0) Logger.Warning($"{RobotController.StateMsg.SerialNumber} - Robot Order xử lí làm mới đường lỗi: Không có edge mới nhận được"); else if (solution.Nodes.Length <= 1) Logger.Warning($"{RobotController.StateMsg.SerialNumber} - Robot Order xử lí làm mới đường lỗi: nodes mới nhận được có số lượng {solution.Nodes.Length}"); else if (solution.Nodes[^1].Id != Nodes[^1].Id) Logger.Warning($"{RobotController.StateMsg.SerialNumber} - Robot Order xử lí làm mới đường lỗi: tuyến đường mới không kết thúc tại đích cũ"); else { List nodes = [..solution.Nodes.Select(n => new NodeDto() { Id = n.Id, Name = n.Name, X = n.X, Y = n.Y, Direction = MapCompute.GetNodeDirection(n.Direction), })]; List edges = [..solution.Edges.Select(e => new EdgeDto() { Id = e.Id, StartNodeId = e.StartNodeId, EndNodeId = e.EndNodeId, ControlPoint1X = e.ControlPoint1X, ControlPoint1Y = e.ControlPoint1Y, ControlPoint2X = e.ControlPoint2X, ControlPoint2Y = e.ControlPoint2Y, TrajectoryDegree = e.TrajectoryDegree, })]; UpdatePath([.. nodes], [.. edges]); } } private async Task RequestInACS(Dictionary newZones) { Guid trafficACSrelaseNodeId = Guid.Empty; foreach (var zone in newZones) { if (zone.Value.Length == 0) trafficACSrelaseNodeId = zone.Key; else { bool requestSuccess = true; foreach (var zoneACS in zone.Value) { if (string.IsNullOrEmpty(zoneACS.Name)) continue; if (CurrentZones.Any(z => z.Id == zoneACS.Id)) continue; var getTrafficACS = await TrafficACS.RequestIn(RobotController.SerialNumber, zoneACS.Name); if (getTrafficACS.IsSuccess && getTrafficACS.Data) CurrentZones.Add(zoneACS); else { requestSuccess = false; break; } } if (requestSuccess) trafficACSrelaseNodeId = zone.Key; else break; } } return trafficACSrelaseNodeId; } private async Task UpdateTraffic() { var trafficManagerGoalIndex = Nodes.FindIndex(n => n.Id == TrafficManagerGoalId); var trafficACSGoalIndex = Nodes.FindIndex(n => n.Id == TrafficACSGoalId); if (trafficManagerGoalIndex != -1 && trafficACSGoalIndex != -1) { var goalIndex = Math.Min(trafficManagerGoalIndex, trafficACSGoalIndex); NodeDto goal = Nodes[goalIndex]; var updatemsg = UpdateGoal(Nodes[goalIndex].Id); if (updatemsg && Nodes[goalIndex].Id != CurrentBaseId) { var publish = await PublishOrder(Nodes[goalIndex].Id); if (!publish) Logger.Warning($"{RobotController.StateMsg.SerialNumber} - Robot Order publish xảy ra lỗi"); } } } private async Task GoOutTrafficACS(Guid lastNodeId) { if (CurrentBaseId == Guid.Empty) return; var goalIndex = Nodes.FindIndex(n => n.Id == CurrentBaseId); if (goalIndex == -1) return; var inNodeIndex = Nodes.FindIndex(n => n.Id == lastNodeId); inNodeIndex = inNodeIndex != -1 ? inNodeIndex : 0; if (goalIndex <= inNodeIndex) return; var baseNodes = Nodes.GetRange(inNodeIndex, goalIndex + 1 - inNodeIndex); var baseZones = PathPlanner.GetZones([.. baseNodes], Zones); var outZones = CurrentZones.Where(z => !baseZones.Any(bz => bz.Id == z.Id)).ToList(); foreach (var zoneACS in outZones) { if (string.IsNullOrEmpty(zoneACS.Name)) continue; var outTrafficACS = await TrafficACS.RequestOut(RobotController.SerialNumber, zoneACS.Name); if (outTrafficACS.IsSuccess && outTrafficACS.Data) CurrentZones.RemoveAll(z => z.Id == zoneACS.Id); } } private Guid GetLastNodeId() { Guid lastNodeId = Guid.Empty; double minDeviationDistance = TrafficACS.DeviationDistance; foreach (var node in Nodes) { var distance = Math.Sqrt(Math.Pow(RobotController.VisualizationMsg.AgvPosition.X - node.X, 2) + Math.Pow(RobotController.VisualizationMsg.AgvPosition.Y - node.Y, 2)); if (distance < minDeviationDistance) { minDeviationDistance = distance; lastNodeId = node.Id; } } return lastNodeId; } private async Task TimerHandler() { try { var lastNodeId = GetLastNodeId(); if (lastNodeId != Guid.Empty && LastNodeId != lastNodeId) LastNodeId = lastNodeId; if (TimerCounter++ > 10) { TimerCounter = 0; if (IsOrderClear()) { if (RobotController.StateMsg.LastNodeId == Nodes[^1].Id.ToString() && GetActionFinished()) { IsCompleted = true; Logger.Info($"{RobotController.StateMsg.SerialNumber} - Robot Order đã hoàn thành tới đích {Nodes[^1].Name}"); } else if (RobotController.StateMsg.LastNodeId != Nodes[^1].Id.ToString() || GetActionFailed()) { Logger.Error($"{RobotController.StateMsg.SerialNumber} - Robot Order kết thúc lỗi. {string.Join(", ", GetError())}"); IsError = true; } } if (!RobotController.IsOnline) { Logger.Error($"{RobotController.StateMsg.SerialNumber} - Robot Order kết thúc: robot mất kết nối"); IsError = true; } if (IsCompleted || IsError || (IsWaittingCancel && IsOrderClear())) { Dispose(); return; } if (!IsWaittingCancel) { await GoOutTrafficACS(LastNodeId); if (!TrafficManager.Enable) { if (TrafficManagerGoalId != Nodes[^1].Id) TrafficManagerGoalId = Nodes[^1].Id; } else { TrafficManager.UpdateInNode(RobotController.SerialNumber, LastNodeId); var trafficSolution = TrafficManager.GetTrafficNode(RobotController.SerialNumber); switch (trafficSolution.State) { case TrafficSolutionState.GiveWay: HandleGiveWayState(trafficSolution); break; case TrafficSolutionState.RefreshPath: HandleRefreshPath(trafficSolution); break; case TrafficSolutionState.Complete: case TrafficSolutionState.Waitting: default: break; } TrafficSolutionState = trafficSolution.State; var goal = Nodes.FirstOrDefault(n => n.Id == trafficSolution.ReleaseNode.Id); if (goal is not null && goal.Id != TrafficManagerGoalId) TrafficManagerGoalId = goal.Id; } if (!TrafficACS.Enable) { if (TrafficACSGoalId != Nodes[^1].Id) TrafficACSGoalId = Nodes[^1].Id; } else { var newZones = TrafficACS.GetZones(LastNodeId, [.. Nodes], Zones); var trafficACSrelaseNodeId = await RequestInACS(newZones); if (trafficACSrelaseNodeId != Guid.Empty && trafficACSrelaseNodeId != TrafficACSGoalId) TrafficACSGoalId = trafficACSrelaseNodeId; } await UpdateTraffic(); } } } catch (Exception ex) { Logger.Error($"{RobotController.StateMsg.SerialNumber} - Robot Order Handler xảy ra lỗi: {ex}"); } } private IEnumerable GetError() { if (RobotController.StateMsg.Errors is not null && RobotController.StateMsg.Errors.Length > 0) { foreach (var error in RobotController.StateMsg.Errors) { yield return $"{error.ErrorType} - {error.ErrorDescription}"; } } yield break; } private bool GetActionFinished() { if (RobotController.StateMsg.ActionStates is not null && RobotController.StateMsg.ActionStates.Length > 0) { var finalActions = OrderMsg.Nodes[^1].Actions; foreach (var actionState in RobotController.StateMsg.ActionStates) { if (finalActions.Any(a => a.ActionId == actionState.ActionId) && actionState.ActionStatus != ActionStatus.FINISHED.ToString()) return false; } } return true; } private bool GetActionFailed() { if (RobotController.StateMsg.ActionStates is not null && RobotController.StateMsg.ActionStates.Length > 0) { var finalActions = OrderMsg.Nodes[^1].Actions; foreach (var actionState in RobotController.StateMsg.ActionStates) { if (finalActions.Any(a => a.ActionId == actionState.ActionId) && actionState.ActionStatus == ActionStatus.FAILED.ToString()) return true; } } return false; } public async Task Cancel(CancellationToken cancellation) { try { IsWaittingCancel = true; while (!cancellation.IsCancellationRequested) { if (IsDisposed) { IsCanceled = true; return true; } if (Timer.Disposed) Dispose(); try { await Task.Delay(500, cancellation); } catch { } } IsWaittingCancel = false; return false; } catch { IsWaittingCancel = false; return false; } } private NavigationPathEdge[] SplitChecking(NodeDto lastNode, NodeDto nearLastNode, EdgeDto edge) { List pathEdges = []; var splitStartPath = PathPlanner.PathSplit([new NodeDto { Id = lastNode.Id, X = lastNode.X, Y = lastNode.Y, }, new NodeDto { Id = nearLastNode.Id, X = nearLastNode.X, Y = nearLastNode.Y, }], [edge], 0.1); if (splitStartPath.IsSuccess && splitStartPath.Data is not null) { int index = 0; double minDistance = double.MaxValue; for (int i = 0; i < splitStartPath.Data.Length; i++) { var distance = Math.Sqrt(Math.Pow(RobotController.VisualizationMsg.AgvPosition.X - splitStartPath.Data[i].X, 2) + Math.Pow(RobotController.VisualizationMsg.AgvPosition.Y - splitStartPath.Data[i].Y, 2)); if (distance < minDistance) { minDistance = distance; index = i; } } for (int i = index; i < splitStartPath.Data.Length - 1; i++) { pathEdges.Add(new() { StartX = splitStartPath.Data[i].X, StartY = splitStartPath.Data[i].Y, EndX = splitStartPath.Data[i + 1].X, EndY = splitStartPath.Data[i + 1].Y, Degree = 1, }); } } return [.. pathEdges]; } private NavigationPathEdge[] GetFullPath() { List pathEdges = []; if (RobotController.StateMsg.NodeStates is not null && RobotController.StateMsg.NodeStates.Length > 0) { var lastNodeIndex = Nodes.FindIndex(n => n.Id == LastNodeId); lastNodeIndex = lastNodeIndex != -1 ? lastNodeIndex : 0; if (lastNodeIndex < Nodes.Count - 1) pathEdges = [.. SplitChecking(Nodes[lastNodeIndex], Nodes[lastNodeIndex + 1], Edges[lastNodeIndex])]; if (lastNodeIndex < Nodes.Count - 2) { var nodes = Nodes.GetRange(lastNodeIndex + 1, Nodes.Count - lastNodeIndex - 1); var edges = Edges.GetRange(lastNodeIndex + 1, nodes.Count - 1); for (int i = 0; i < nodes.Count - 1; i++) { pathEdges.Add(new() { StartX = nodes[i].X, StartY = nodes[i].Y, EndX = nodes[i + 1].X, EndY = nodes[i + 1].Y, ControlPoint1X = edges[i].ControlPoint1X, ControlPoint1Y = edges[i].ControlPoint1Y, ControlPoint2X = edges[i].ControlPoint2X, ControlPoint2Y = edges[i].ControlPoint2Y, Degree = edges[i].TrajectoryDegree == MapShares.Enums.TrajectoryDegree.One ? 1 : edges[i].TrajectoryDegree == MapShares.Enums.TrajectoryDegree.Two ? 2 : 3, }); } } } return [.. pathEdges]; } private NavigationPathEdge[] GetBasePath() { List pathEdges = []; if (RobotController.StateMsg.NodeStates is not null && RobotController.StateMsg.NodeStates.Length > 0) { var lastNodeIndex = Nodes.FindIndex(n => n.Id == LastNodeId); lastNodeIndex = lastNodeIndex != -1 ? lastNodeIndex : 0; var baseNodeIndex = Nodes.FindIndex(n => n.Id == CurrentBaseId); baseNodeIndex = baseNodeIndex != -1 ? baseNodeIndex : Nodes.Count - 1; if (lastNodeIndex < baseNodeIndex) { if (lastNodeIndex < Nodes.Count - 1) pathEdges = [.. SplitChecking(Nodes[lastNodeIndex], Nodes[lastNodeIndex + 1], Edges[lastNodeIndex])]; if (lastNodeIndex < Nodes.Count - 2 && lastNodeIndex < baseNodeIndex - 1) { var nodes = Nodes.GetRange(lastNodeIndex + 1, baseNodeIndex - lastNodeIndex); if (nodes.Count > 1) { var edges = Edges.GetRange(lastNodeIndex + 1, nodes.Count - 1); for (int i = 0; i < nodes.Count - 1; i++) { pathEdges.Add(new() { StartX = nodes[i].X, StartY = nodes[i].Y, EndX = nodes[i + 1].X, EndY = nodes[i + 1].Y, ControlPoint1X = edges[i].ControlPoint1X, ControlPoint1Y = edges[i].ControlPoint1Y, ControlPoint2X = edges[i].ControlPoint2X, ControlPoint2Y = edges[i].ControlPoint2Y, Degree = edges[i].TrajectoryDegree == MapShares.Enums.TrajectoryDegree.One ? 1 : edges[i].TrajectoryDegree == MapShares.Enums.TrajectoryDegree.Two ? 2 : 3, }); } } } } } return [.. pathEdges]; } private bool IsOrderClear() => RobotController.StateMsg.NodeStates.Length == 0 && RobotController.StateMsg.EdgeStates.Length == 0; public void CreateComledted() { IsCompleted = true; IsCanceled = false; IsError = false; } public void Dispose() { IsDisposed = true; Timer.Dispose(); TrafficManager.DeleteAgent(RobotController.SerialNumber); GC.SuppressFinalize(this); } }