first commit -push
This commit is contained in:
dungtt
750
RobotNet.RobotManager/Services/Robot/RobotController.cs
Normal file
750
RobotNet.RobotManager/Services/Robot/RobotController.cs
Normal file
@@ -0,0 +1,750 @@
|
||||
using Microsoft.EntityFrameworkCore;
|
||||
using RobotNet.MapShares;
|
||||
using RobotNet.MapShares.Dtos;
|
||||
using RobotNet.MapShares.Enums;
|
||||
using RobotNet.RobotManager.Data;
|
||||
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;
|
||||
using RobotNet.RobotShares.VDA5050.Factsheet;
|
||||
using RobotNet.RobotShares.VDA5050.FactsheetExtend;
|
||||
using RobotNet.RobotShares.VDA5050.Order;
|
||||
using RobotNet.RobotShares.VDA5050.State;
|
||||
using RobotNet.RobotShares.VDA5050.Type;
|
||||
using RobotNet.RobotShares.VDA5050.Visualization;
|
||||
using RobotNet.Shares;
|
||||
using System.Text.Json;
|
||||
using Action = RobotNet.RobotShares.VDA5050.InstantAction.Action;
|
||||
using NodePosition = RobotNet.RobotShares.VDA5050.Order.NodePosition;
|
||||
|
||||
namespace RobotNet.RobotManager.Services.Robot;
|
||||
|
||||
public class RobotController(string robotid,
|
||||
string Manufacturer,
|
||||
string Version,
|
||||
NavigationType NavigationType,
|
||||
IServiceProvider ServiceProvider,
|
||||
Func<string, string, bool> FuncPub) : IRobotController, IDisposable
|
||||
{
|
||||
public string SerialNumber { get; } = robotid;
|
||||
public bool IsOnline { get; set; }
|
||||
public bool IsWorking => (RobotOrder is not null && RobotOrder.IsProcessing) || StateMsg.NodeStates.Length != 0 || StateMsg.EdgeStates.Length != 0;
|
||||
public string State => GetState();
|
||||
public string[] CurrentZones => RobotOrder is null ? [] : [.. RobotOrder.CurrentZones.Select(z => z.Name)];
|
||||
public RobotOrderDto OrderState => GetOrderState();
|
||||
public RobotActionDto[] ActionStates => GetActionsState();
|
||||
public AutoResetEvent RobotUpdated { get; set; } = new(false);
|
||||
public StateMsg StateMsg { get; set; } = new();
|
||||
public VisualizationMsg VisualizationMsg { get; set; } = new();
|
||||
public FactSheetMsg FactSheetMsg { get; set; } = new();
|
||||
public FactsheetExtendMsg FactsheetExtendMsg { get; set; } = new();
|
||||
public NavigationPathEdge[] BasePath => RobotOrder is null ? [] : RobotOrder.BasePath;
|
||||
public NavigationPathEdge[] FullPath => RobotOrder is null ? [] : RobotOrder.FullPath;
|
||||
|
||||
private readonly LoggerController<RobotController> Logger = ServiceProvider.GetRequiredService<LoggerController<RobotController>>();
|
||||
private readonly TimeSpan WaittingRobotFeedbackTime = TimeSpan.FromSeconds(10);
|
||||
private RobotOrder? RobotOrder;
|
||||
|
||||
private CancellationTokenSource? CancelRandom;
|
||||
|
||||
public void Log(string message, LogLevel level = LogLevel.Information)
|
||||
{
|
||||
switch (level)
|
||||
{
|
||||
case LogLevel.Trace: Logger.Trace($"{SerialNumber} - {message}"); break;
|
||||
case LogLevel.Error: Logger.Error($"{SerialNumber} - {message}"); break;
|
||||
case LogLevel.Warning: Logger.Warning($"{SerialNumber} - {message}"); break;
|
||||
case LogLevel.Critical: Logger.Critical($"{SerialNumber} - {message}"); break;
|
||||
case LogLevel.Information: Logger.Info($"{SerialNumber} - {message}"); break;
|
||||
case LogLevel.Debug: Logger.Debug($"{SerialNumber} - {message}"); break;
|
||||
default: Logger.Debug($"{SerialNumber} - {message}"); break;
|
||||
}
|
||||
}
|
||||
|
||||
public async Task<MessageResult> CancelOrder()
|
||||
{
|
||||
try
|
||||
{
|
||||
if (StateMsg.NodeStates.Length != 0 || StateMsg.EdgeStates.Length != 0)
|
||||
{
|
||||
Action cancelOrderAction = new()
|
||||
{
|
||||
ActionDescription = "Yêu cầu hủy nhiệm vụ hiện tại",
|
||||
ActionParameters = [new RobotShares.VDA5050.InstantAction.ActionParameter()
|
||||
{
|
||||
Key = "ORDER_ID",
|
||||
Value = StateMsg.OrderId,
|
||||
}],
|
||||
ActionType = ActionType.cancelOrder.ToString(),
|
||||
BlockingType = RobotShares.VDA5050.InstantAction.BlockingType.NONE.ToString(),
|
||||
};
|
||||
var pubAction = await InstantAction(cancelOrderAction, true);
|
||||
if (!pubAction.IsSuccess) return new(false, pubAction.Message);
|
||||
}
|
||||
if (CancelRandom is not null && !CancelRandom.IsCancellationRequested) CancelRandom.Cancel();
|
||||
if (RobotOrder is not null && RobotOrder.IsProcessing)
|
||||
{
|
||||
CancellationTokenSource cancellationToken = new();
|
||||
cancellationToken.CancelAfter(TimeSpan.FromSeconds(10));
|
||||
var waitCancel = await RobotOrder.Cancel(cancellationToken.Token);
|
||||
if (!waitCancel) return new(false, "Robot không kết thúc order");
|
||||
}
|
||||
return new(true);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Logger.Warning($"{SerialNumber} Cancel Order logs: {ex.Message}");
|
||||
return new(false, $"{SerialNumber} Cancel Order logs: {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
public async Task<MessageResult> CancelAction()
|
||||
{
|
||||
try
|
||||
{
|
||||
Action cancelOrderAction = new()
|
||||
{
|
||||
ActionDescription = "Yêu cầu hủy Action hiện tại",
|
||||
ActionParameters = [new RobotShares.VDA5050.InstantAction.ActionParameter()
|
||||
{
|
||||
Key = "ORDER_ID",
|
||||
Value = SerialNumber,
|
||||
}],
|
||||
ActionType = ActionType.cancelOrder.ToString(),
|
||||
BlockingType = RobotShares.VDA5050.InstantAction.BlockingType.NONE.ToString(),
|
||||
};
|
||||
var pubAction = await InstantAction(cancelOrderAction, true);
|
||||
return new(pubAction.IsSuccess, pubAction.Message);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Logger.Warning($"{SerialNumber} Cancel Action logs: {ex.Message}");
|
||||
return new(false, $"{SerialNumber} Cancel Action logs: {ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
public void Initialize(double x, double y, double theta)
|
||||
{
|
||||
throw new NotImplementedException();
|
||||
}
|
||||
|
||||
public async Task<MessageResult<string>> InstantAction(Action action, bool waittingFisished)
|
||||
{
|
||||
CancellationTokenSource CancellationToken = new();
|
||||
CancellationToken.CancelAfter(WaittingRobotFeedbackTime);
|
||||
bool IsFeedback = false;
|
||||
int RepeatCounter = 0;
|
||||
try
|
||||
{
|
||||
action.ActionId = Guid.NewGuid().ToString();
|
||||
var instantActionsMsg = new RobotNet.RobotShares.VDA5050.InstantAction.InstantActionsMsg()
|
||||
{
|
||||
HeaderId = 1,
|
||||
Manufacturer = Manufacturer,
|
||||
Version = Version,
|
||||
Timestamp = DateTime.Now.ToString("yyyy-MM-ddTHH:mm:ss.fffZ"),
|
||||
SerialNumber = SerialNumber,
|
||||
Actions = [action],
|
||||
};
|
||||
var pubInstantAction = FuncPub.Invoke(VDA5050Topic.InstantActions, JsonSerializer.Serialize(instantActionsMsg, JsonOptionExtends.Write));
|
||||
if (!pubInstantAction)
|
||||
{
|
||||
string msg = $"Gửi Action xuống cho robot không thành công: {action.ActionType}";
|
||||
Log(msg, LogLevel.Warning);
|
||||
return new(false, msg);
|
||||
}
|
||||
while (!IsFeedback)
|
||||
{
|
||||
if (StateMsg is not null && StateMsg.ActionStates is not null)
|
||||
{
|
||||
var actionstate = StateMsg.ActionStates.FirstOrDefault(ac => ac.ActionId == action.ActionId);
|
||||
if (actionstate is not null)
|
||||
{
|
||||
IsFeedback = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if(CancellationToken.IsCancellationRequested)
|
||||
{
|
||||
if (RepeatCounter++ == 0)
|
||||
{
|
||||
await Task.Delay(1000);
|
||||
CancellationToken = new();
|
||||
CancellationToken.CancelAfter(WaittingRobotFeedbackTime);
|
||||
pubInstantAction = FuncPub.Invoke(VDA5050Topic.InstantActions, JsonSerializer.Serialize(instantActionsMsg, JsonOptionExtends.Write));
|
||||
if (!pubInstantAction)
|
||||
{
|
||||
string msg = $"Gửi Action xuống cho robot không thành công: {action.ActionType}";
|
||||
Log(msg, LogLevel.Warning);
|
||||
return new(false, msg);
|
||||
}
|
||||
}
|
||||
else return new(false, $"Robot không đồng ý thực hiện action: {action.ActionType}");
|
||||
}
|
||||
await Task.Delay(500);
|
||||
}
|
||||
if (waittingFisished)
|
||||
{
|
||||
while (!CancellationToken.IsCancellationRequested)
|
||||
{
|
||||
if (StateMsg is not null)
|
||||
{
|
||||
if (StateMsg.ActionStates is not null)
|
||||
{
|
||||
var actionCancelOrder = StateMsg.ActionStates.FirstOrDefault(a => a.ActionId == action.ActionId);
|
||||
if (actionCancelOrder is not null)
|
||||
{
|
||||
if (ActionFinish(actionCancelOrder.ActionStatus)) return new(true);
|
||||
if (ActionFailed(actionCancelOrder.ActionStatus))
|
||||
{
|
||||
string msg = $"Robot trả về thực hiện action lỗi: {action.ActionType}";
|
||||
Log(msg, LogLevel.Warning);
|
||||
return new(false, msg);
|
||||
}
|
||||
}
|
||||
}
|
||||
if (GetErrorLevel() != ErrorLevel.NONE)
|
||||
{
|
||||
var errorStr = StateMsg.Errors.Select(error => error.ErrorType).ToArray();
|
||||
string msg = $"Robot xảy ra lỗi sau khi gọi thực hiện action {action.ActionType}";
|
||||
Log($"{msg} - {string.Join(",", errorStr)}", LogLevel.Warning);
|
||||
return new(false, msg);
|
||||
}
|
||||
}
|
||||
if (CancellationToken.IsCancellationRequested) return new(false, "Robot đang thực hiện Action nhưng đã quá thời gian timeout 10s");
|
||||
await Task.Delay(500);
|
||||
}
|
||||
}
|
||||
if (CancellationToken.IsCancellationRequested) throw new Exception();
|
||||
return new(true) { Data = action.ActionId };
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
string msg = $"Có lỗi xảy ra trong quá trình gửi action : {ex.Message}";
|
||||
Log(msg, LogLevel.Warning);
|
||||
return new(false, msg);
|
||||
}
|
||||
}
|
||||
|
||||
public MessageResult MoveStraight(double x, double y)
|
||||
{
|
||||
return new(false, "Chức năng này hiện không khả dụng");
|
||||
}
|
||||
|
||||
public async Task<MessageResult> MoveToNode(string goalName, IDictionary<string, IEnumerable<RobotShares.VDA5050.InstantAction.Action>>? actions = null, double? lastAngle = null)
|
||||
{
|
||||
try
|
||||
{
|
||||
if (IsWorking) return new(false, "Robot đang thực hiện nhiệm vụ");
|
||||
|
||||
using var scope = ServiceProvider.CreateScope();
|
||||
var PathPlanner = scope.ServiceProvider.GetRequiredService<PathPlanner>();
|
||||
var robotDb = scope.ServiceProvider.GetRequiredService<RobotEditorDbContext>();
|
||||
var MapManager = scope.ServiceProvider.GetRequiredService<MapManager>();
|
||||
var TrafficManager = scope.ServiceProvider.GetRequiredService<TrafficManager>();
|
||||
var TrafficACS = scope.ServiceProvider.GetRequiredService<TrafficACS>();
|
||||
var orderLogger = scope.ServiceProvider.GetRequiredService<LoggerController<RobotOrder>>();
|
||||
|
||||
var robot = await robotDb.Robots.FirstOrDefaultAsync(r => r.RobotId == SerialNumber) ??
|
||||
throw new Exception($"Không tìm thấy robot {SerialNumber} trong kho dữ liệu");
|
||||
var robotModel = await robotDb.RobotModels.FirstOrDefaultAsync(r => r.Id == robot.ModelId) ??
|
||||
throw new Exception($"Không tìm thấy robot model {robot.ModelId} trong kho dữ liệu");
|
||||
|
||||
var path = await PathPlanner.Planning(VisualizationMsg.AgvPosition.X, VisualizationMsg.AgvPosition.Y, VisualizationMsg.AgvPosition.Theta, NavigationType, robot.MapId, goalName);
|
||||
if (!path.IsSuccess) return new(false, path.Message);
|
||||
if(path.IsSuccess && path.Data.Nodes.Length < 2)
|
||||
{
|
||||
RobotOrder?.CreateComledted();
|
||||
return new(true, "");
|
||||
}
|
||||
if (path.Data.Nodes is null || path.Data.Edges is null || path.Data.Edges.Length == 0 || path.Data.Nodes.Length < 2)
|
||||
return new(false, $"Đường dẫn tới đích {goalName} từ [{VisualizationMsg.AgvPosition.X} - {VisualizationMsg.AgvPosition.Y} - {VisualizationMsg.AgvPosition.Theta}] không tồn tại");
|
||||
|
||||
if (lastAngle != null)
|
||||
{
|
||||
path.Data.Nodes[^1].Theta = lastAngle.Value;
|
||||
}
|
||||
|
||||
var mapData = await MapManager.GetMapData(robot.MapId);
|
||||
if (!mapData.IsSuccess) return new(false, mapData.Message);
|
||||
if (mapData is null || mapData.Data is null) return new(false, "Không thể tìm thấy bản đồ chứa robot.");
|
||||
var nodeInZones = await PathPlanner.GetZones(robot.MapId, path.Data.Nodes);
|
||||
if (!nodeInZones.IsSuccess) return new(false, mapData.Message);
|
||||
var order = await Order([.. path.Data.Nodes], [.. path.Data.Edges], mapData.Data, actions, TrafficManager.Enable);
|
||||
if (order.IsSuccess && order.Data is not null)
|
||||
{
|
||||
int counter = 0;
|
||||
while (counter++ < 2)
|
||||
{
|
||||
var createAgent = TrafficManager.CreateAgent(robot.MapId, this, new()
|
||||
{
|
||||
|
||||
NavigationType = robotModel.NavigationType,
|
||||
Length = robotModel.Length,
|
||||
Width = robotModel.Width,
|
||||
NavigationPointX = robotModel.OriginX,
|
||||
NavigationPointY = robotModel.OriginY,
|
||||
},
|
||||
[..path.Data.Nodes.Select(n => new TrafficNodeDto()
|
||||
{
|
||||
Id = n.Id,
|
||||
Name = n.Name,
|
||||
X = n.X,
|
||||
Y = n.Y,
|
||||
Direction = MapCompute.GetRobotDirection(n.Direction),
|
||||
})], [..path.Data.Edges.Select(n => new TrafficEdgeDto()
|
||||
{
|
||||
Id = n.Id,
|
||||
StartNodeId = n.StartNodeId,
|
||||
EndNodeId = n.EndNodeId,
|
||||
TrajectoryDegree = n.TrajectoryDegree,
|
||||
ControlPoint1X = n.ControlPoint1X,
|
||||
ControlPoint1Y = n.ControlPoint1Y,
|
||||
ControlPoint2X = n.ControlPoint2X,
|
||||
ControlPoint2Y = n.ControlPoint2Y,
|
||||
})]);
|
||||
if (createAgent.IsSuccess) break;
|
||||
Logger.Warning($"{SerialNumber} - Không thể tạo traffic agent: {createAgent.Message}");
|
||||
if (counter > 1 && !createAgent.IsSuccess)
|
||||
{
|
||||
var cancel = await CancelOrder();
|
||||
if (!cancel.IsSuccess) Logger.Warning($"{SerialNumber} - Không thể hủy bỏ nhiệm vụ đã giao: {cancel.Message}");
|
||||
return new(false, $"Không thể tạo traffic agent: {createAgent.Message} - {(cancel.IsSuccess ? "Đã hủy order" : $"Không thể hủy order: {cancel.Message}")} ");
|
||||
}
|
||||
}
|
||||
RobotOrder = new RobotOrder([.. path.Data.Nodes], [.. path.Data.Edges], nodeInZones.Data ?? [], actions ?? new Dictionary<string, IEnumerable<Action>>(),
|
||||
order.Data, this, NavigationType, TrafficManager, TrafficACS, MapManager, orderLogger, Order)
|
||||
{
|
||||
CurrentZones = RobotOrder is null ? [] : RobotOrder.CurrentZones,
|
||||
};
|
||||
|
||||
return new(true);
|
||||
}
|
||||
else return new(order.IsSuccess, order.Message);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
string msg = $"Có lỗi xảy ra trong quá trình gửi order to Node {goalName} : {ex.Message}";
|
||||
Log(msg, LogLevel.Warning);
|
||||
return new(false, msg);
|
||||
}
|
||||
}
|
||||
|
||||
public MessageResult Rotate(double angle)
|
||||
{
|
||||
return new(false, "Chức năng này hiện không khả dụng");
|
||||
}
|
||||
|
||||
private async Task<MessageResult> Order(OrderMsg orderMsg)
|
||||
{
|
||||
CancellationTokenSource CancellationExitToken = new();
|
||||
CancellationExitToken.CancelAfter(WaittingRobotFeedbackTime);
|
||||
try
|
||||
{
|
||||
var pubOrder = FuncPub.Invoke(VDA5050Topic.Order, JsonSerializer.Serialize(orderMsg, JsonOptionExtends.Write));
|
||||
if (!pubOrder)
|
||||
{
|
||||
string msg = $"Gửi Order xuống cho robot không thành công: {orderMsg.OrderId}";
|
||||
Log(msg, LogLevel.Warning);
|
||||
return new(false, msg);
|
||||
}
|
||||
while (!CancellationExitToken.IsCancellationRequested)
|
||||
{
|
||||
if (StateMsg is not null)
|
||||
{
|
||||
if (StateMsg.OrderId == orderMsg.OrderId && IsWorking && StateMsg.OrderUpdateId == orderMsg.OrderUpdateId) break;
|
||||
}
|
||||
|
||||
await Task.Delay(500, CancellationExitToken.Token);
|
||||
}
|
||||
if (CancellationExitToken.IsCancellationRequested) throw new Exception();
|
||||
return new(true);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
string msg = $"Có lỗi xảy ra trong quá trình gửi order : {ex.Message}";
|
||||
if (CancellationExitToken.IsCancellationRequested) msg = $"{SerialNumber} - Robot không đồng ý thực hiện order: {orderMsg.OrderId}";
|
||||
|
||||
Log(msg, LogLevel.Warning);
|
||||
return new(false, msg);
|
||||
}
|
||||
}
|
||||
|
||||
private static List<Action> ConvertAction(string actions, ActionDto[] mapAction)
|
||||
{
|
||||
List<Action> Actions = [];
|
||||
var ActionIds = !string.IsNullOrEmpty(actions) ? JsonSerializer.Deserialize<Guid[]>(actions) : [];
|
||||
if (ActionIds is not null)
|
||||
{
|
||||
foreach (var actionid in ActionIds)
|
||||
{
|
||||
var actionDb = mapAction.FirstOrDefault(x => x.Id == actionid);
|
||||
if (actionDb is not null)
|
||||
{
|
||||
var vdaAction = JsonSerializer.Deserialize<Action>(actionDb.Content, JsonOptionExtends.Read);
|
||||
if (vdaAction is not null) Actions.Add(vdaAction);
|
||||
}
|
||||
}
|
||||
}
|
||||
return Actions;
|
||||
}
|
||||
|
||||
private static Edge? ConvertToOrderEdge(EdgeDto edge, MapDataDto map)
|
||||
{
|
||||
var startNode = map.Nodes.FirstOrDefault(n => n.Id == edge.StartNodeId);
|
||||
var endNode = map.Nodes.FirstOrDefault(n => n.Id == edge.EndNodeId);
|
||||
var dataEdge = map.Edges.FirstOrDefault(e => e.Id == edge.Id);
|
||||
|
||||
if (startNode is null || endNode is null || dataEdge is null) return null;
|
||||
|
||||
List<ControlPoint> ControlPoints = [];
|
||||
ControlPoints.Add(new()
|
||||
{
|
||||
X = startNode.X,
|
||||
Y = startNode.Y,
|
||||
Weight = 1,
|
||||
});
|
||||
if (dataEdge.TrajectoryDegree == TrajectoryDegree.Two)
|
||||
{
|
||||
ControlPoints.Add(new()
|
||||
{
|
||||
X = dataEdge.ControlPoint1X,
|
||||
Y = dataEdge.ControlPoint1Y,
|
||||
Weight = 1,
|
||||
});
|
||||
}
|
||||
else if (dataEdge.TrajectoryDegree == TrajectoryDegree.Three)
|
||||
{
|
||||
ControlPoint controlPoint1 = new();
|
||||
ControlPoint controlPoint2 = new();
|
||||
if (dataEdge.StartNodeId == edge.StartNodeId)
|
||||
{
|
||||
controlPoint1.X = dataEdge.ControlPoint1X;
|
||||
controlPoint1.Y = dataEdge.ControlPoint1Y;
|
||||
controlPoint1.Weight = 1;
|
||||
controlPoint2.X = dataEdge.ControlPoint2X;
|
||||
controlPoint2.Y = dataEdge.ControlPoint2Y;
|
||||
controlPoint2.Weight = 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
controlPoint2.X = dataEdge.ControlPoint1X;
|
||||
controlPoint2.Y = dataEdge.ControlPoint1Y;
|
||||
controlPoint2.Weight = 1;
|
||||
controlPoint1.X = dataEdge.ControlPoint2X;
|
||||
controlPoint1.Y = dataEdge.ControlPoint2Y;
|
||||
controlPoint1.Weight = 1;
|
||||
}
|
||||
ControlPoints.Add(controlPoint1);
|
||||
ControlPoints.Add(controlPoint2);
|
||||
}
|
||||
ControlPoints.Add(new()
|
||||
{
|
||||
X = endNode.X,
|
||||
Y = endNode.Y,
|
||||
Weight = 1,
|
||||
});
|
||||
|
||||
var result = new Edge()
|
||||
{
|
||||
EdgeId = edge.Id.ToString(),
|
||||
EdgeDescription = $"{startNode.Name} - {endNode.Name}",
|
||||
EndNodeId = edge.EndNodeId.ToString(),
|
||||
StartNodeId = edge.StartNodeId.ToString(),
|
||||
SequenceId = 1,
|
||||
Direction = dataEdge.DirectionAllowed.ToString(),
|
||||
Orientation = 0,
|
||||
Released = true,
|
||||
Length = MapEditorHelper.GetEdgeLength(new()
|
||||
{
|
||||
X1 = startNode.X,
|
||||
X2 = endNode.X,
|
||||
Y1 = startNode.Y,
|
||||
Y2 = endNode.Y,
|
||||
TrajectoryDegree = dataEdge.TrajectoryDegree,
|
||||
ControlPoint1X = dataEdge.ControlPoint1X,
|
||||
ControlPoint1Y = dataEdge.ControlPoint1Y,
|
||||
ControlPoint2X = dataEdge.ControlPoint2X,
|
||||
ControlPoint2Y = dataEdge.ControlPoint2Y,
|
||||
}),
|
||||
MaxHeight = dataEdge.MaxHeight,
|
||||
MaxRotationSpeed = dataEdge.MaxRotationSpeed,
|
||||
OrientationType = "",
|
||||
MaxSpeed = dataEdge.MaxSpeed,
|
||||
MinHeight = dataEdge.MinHeight,
|
||||
RotationAllowed = dataEdge.RotationAllowed,
|
||||
Actions = [.. ConvertAction(dataEdge.Actions, [.. map.Actions])],
|
||||
Trajectory = new()
|
||||
{
|
||||
Degree = dataEdge.TrajectoryDegree == TrajectoryDegree.One ? 1 : dataEdge.TrajectoryDegree == TrajectoryDegree.Two ? 2 : 3,
|
||||
KnotVector = dataEdge.TrajectoryDegree == TrajectoryDegree.One ? [0, 0, 1, 1] : dataEdge.TrajectoryDegree == TrajectoryDegree.Two ? [0, 0, 0, 1, 1, 1] : [0, 0, 0, 0, 1, 1, 1, 1],
|
||||
ControlPoints = [.. ControlPoints],
|
||||
},
|
||||
};
|
||||
return result;
|
||||
}
|
||||
|
||||
public static void CreateOrderMsg(ref OrderMsg orderMsgTemplate, NodeDto[] nodes, EdgeDto[] edges, MapDataDto map, IDictionary<string, IEnumerable<Action>> actions, NavigationType navigationType, bool trafficEnable)
|
||||
{
|
||||
if (nodes.Length <= 1 || edges.Length < 1) throw new ArgumentException("Dữ liệu đường đi không hợp lệ");
|
||||
List<Node> OrderNodes = [];
|
||||
List<Edge> OrderEdges = [];
|
||||
for (int i = 0; i < nodes.Length; i++)
|
||||
{
|
||||
var nodeDb = map.Nodes.FirstOrDefault(n => n.Id == nodes[i].Id);
|
||||
if (nodeDb is null)
|
||||
{
|
||||
if (i != 0) throw new ArgumentException("Đường đi tồn tại node không hợp lệ");
|
||||
|
||||
var angleForward = Math.Atan2(nodes[1].Y - nodes[0].Y, nodes[1].X - nodes[0].X);
|
||||
var angleBackward = Math.Atan2(nodes[0].Y - nodes[1].Y, nodes[0].X - nodes[1].X);
|
||||
OrderNodes.Add(new()
|
||||
{
|
||||
NodeId = nodes[i].Id.ToString(),
|
||||
Released = true,
|
||||
SequenceId = i,
|
||||
NodeDescription = nodes[i].Name ?? "",
|
||||
NodePosition = new NodePosition()
|
||||
{
|
||||
X = nodes[i].X,
|
||||
Y = nodes[i].Y,
|
||||
Theta = navigationType == NavigationType.OmniDrive ? 10 : nodes[i].Direction == Direction.FORWARD ? angleForward : angleBackward,
|
||||
MapDescription = map.Name,
|
||||
MapId = map.Id.ToString(),
|
||||
AllowedDeviationTheta = nodes[i].AllowedDeviationTheta,
|
||||
AllowedDeviationXY = nodes[i].AllowedDeviationXy,
|
||||
},
|
||||
Actions = actions.TryGetValue(nodes[i].Name, out IEnumerable<Action>? anoCustomActions) ? [.. anoCustomActions] : [],
|
||||
});
|
||||
continue;
|
||||
}
|
||||
|
||||
double angle;
|
||||
if (i == 0)
|
||||
{
|
||||
var nearNode = MapEditorHelper.GetNearByNode(nodes[0], nodes[1], edges[0], 0.01);
|
||||
var angleForward = Math.Atan2(nearNode.Y - nodes[0].Y, nearNode.X - nodes[0].X);
|
||||
var angleBackward = Math.Atan2(nodes[0].Y - nearNode.Y, nodes[0].X - nearNode.X);
|
||||
angle = nodes[i].Direction == Direction.FORWARD ? angleForward : angleBackward;
|
||||
}
|
||||
else
|
||||
{
|
||||
var nearNode = MapEditorHelper.GetNearByNode(nodes[i], nodes[i - 1], edges[i - 1], 0.01);
|
||||
var angleForward = Math.Atan2(nodeDb.Y - nearNode.Y, nodeDb.X - nearNode.X);
|
||||
var angleBackward = Math.Atan2(nearNode.Y - nodeDb.Y, nearNode.X - nodeDb.X);
|
||||
|
||||
if (nodes[i].Direction == nodes[i - 1].Direction) angle = nodes[i].Direction == Direction.FORWARD ? angleForward : angleBackward;
|
||||
else angle = nodes[i - 1].Direction == Direction.FORWARD ? angleForward : angleBackward;
|
||||
}
|
||||
|
||||
List<Action> nodeActions = ConvertAction(nodeDb.Actions, [.. map.Actions]);
|
||||
if (actions.TryGetValue(nodes[i].Name, out IEnumerable<Action>? customActions) && customActions is not null)
|
||||
{
|
||||
nodeActions.AddRange(customActions);
|
||||
}
|
||||
OrderNodes.Add(new()
|
||||
{
|
||||
NodeId = nodes[i].Id.ToString(),
|
||||
Released = i == 0 || !trafficEnable,
|
||||
SequenceId = i,
|
||||
NodeDescription = nodes[i].Name ?? "",
|
||||
NodePosition = new NodePosition()
|
||||
{
|
||||
X = nodeDb.X,
|
||||
Y = nodeDb.Y,
|
||||
Theta = i == (nodes.Length - 1) ? (nodeDb.Theta * Math.PI / 180) : (navigationType == NavigationType.OmniDrive ? 10 : angle),
|
||||
MapDescription = map.Name,
|
||||
MapId = map.Id.ToString(),
|
||||
AllowedDeviationTheta = nodeDb.AllowedDeviationTheta,
|
||||
AllowedDeviationXY = nodeDb.AllowedDeviationXy,
|
||||
},
|
||||
Actions = [.. nodeActions],
|
||||
});
|
||||
}
|
||||
foreach (var edge in edges)
|
||||
{
|
||||
var orderEdge = ConvertToOrderEdge(edge, map);
|
||||
if (orderEdge is null)
|
||||
{
|
||||
if (edge == edges.First())
|
||||
{
|
||||
orderEdge = new Edge()
|
||||
{
|
||||
EdgeId = edge.Id.ToString(),
|
||||
StartNodeId = edge.StartNodeId.ToString(),
|
||||
EndNodeId = edge.EndNodeId.ToString(),
|
||||
EdgeDescription = $"{edge.StartNodeId} - {edge.EndNodeId}",
|
||||
Direction = edge.DirectionAllowed.ToString(),
|
||||
Orientation = 0,
|
||||
Released = false,
|
||||
Length = 0,
|
||||
MaxRotationSpeed = 0.5,
|
||||
OrientationType = "",
|
||||
MaxSpeed = 0.5,
|
||||
MinHeight = 0,
|
||||
RotationAllowed = true,
|
||||
Actions = [.. ConvertAction(edge.Actions, [.. map.Actions])],
|
||||
Trajectory = new()
|
||||
{
|
||||
Degree = 1,
|
||||
KnotVector = [0, 0, 1, 1],
|
||||
ControlPoints = [new()
|
||||
{
|
||||
X = nodes[0].X,
|
||||
Y = nodes[0].Y,
|
||||
Weight = 1,
|
||||
},
|
||||
new()
|
||||
{
|
||||
X = nodes[1].X,
|
||||
Y = nodes[1].Y,
|
||||
Weight = 1,
|
||||
}],
|
||||
},
|
||||
};
|
||||
}
|
||||
else throw new ArgumentException("Đường đi tồn tại edge không hợp lệ");
|
||||
}
|
||||
orderEdge.SequenceId = Array.IndexOf(edges, edge);
|
||||
orderEdge.Released = !trafficEnable;
|
||||
OrderEdges.Add(orderEdge);
|
||||
}
|
||||
orderMsgTemplate.Nodes = [.. OrderNodes];
|
||||
orderMsgTemplate.Edges = [.. OrderEdges];
|
||||
}
|
||||
|
||||
private async Task<MessageResult<OrderMsg?>> Order(List<NodeDto> nodes, List<EdgeDto> edges, MapDataDto map, IDictionary<string, IEnumerable<Action>>? actions, bool trafficManager)
|
||||
{
|
||||
try
|
||||
{
|
||||
var OrderMsg = new OrderMsg()
|
||||
{
|
||||
HeaderId = 1,
|
||||
Manufacturer = Manufacturer,
|
||||
Version = Version,
|
||||
Timestamp = DateTime.Now.ToString("yyyy-MM-ddTHH:mm:ss.fffZ"),
|
||||
SerialNumber = SerialNumber,
|
||||
OrderId = Guid.NewGuid().ToString(),
|
||||
OrderUpdateId = 1,
|
||||
ZoneSetId = map.Name ?? "",
|
||||
};
|
||||
CreateOrderMsg(ref OrderMsg, [.. nodes], [.. edges], map, actions ?? new Dictionary<string, IEnumerable<Action>>(), NavigationType, trafficManager);
|
||||
var pubOrder = await Order(OrderMsg);
|
||||
if (pubOrder.IsSuccess) return new(true) { Data = OrderMsg };
|
||||
return new(false, pubOrder.Message);
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
string msg = "Có lỗi xảy ra trong quá trình tạo Order";
|
||||
Log($"{msg}: {ex.Message}", LogLevel.Warning);
|
||||
return new(true, msg);
|
||||
}
|
||||
}
|
||||
|
||||
private string GetState()
|
||||
{
|
||||
if (StateMsg.Information is not null)
|
||||
{
|
||||
var RobotGeneral = StateMsg.Information.FirstOrDefault(info => info.InfoType == InformationType.robot_general.ToString());
|
||||
if (RobotGeneral is not null)
|
||||
{
|
||||
var references = RobotGeneral.InfoReferences.FirstOrDefault(key => key.ReferenceKey == InformationReferencesKey.robot_state.ToString());
|
||||
if (references is not null) return references.ReferenceValue;
|
||||
}
|
||||
}
|
||||
return "";
|
||||
}
|
||||
|
||||
private RobotOrderDto GetOrderState()
|
||||
{
|
||||
return new()
|
||||
{
|
||||
OrderId = StateMsg.OrderId,
|
||||
IsCompleted = (RobotOrder is not null && RobotOrder.IsCompleted) || RobotOrder is null,
|
||||
IsError = RobotOrder is not null && RobotOrder.IsError,
|
||||
IsProcessing = RobotOrder is not null && RobotOrder.IsProcessing,
|
||||
IsCanceled = RobotOrder is not null && RobotOrder.IsCanceled,
|
||||
Errors = RobotOrder is not null ? RobotOrder.Errors : [],
|
||||
};
|
||||
}
|
||||
|
||||
private RobotActionDto[] GetActionsState()
|
||||
{
|
||||
List<RobotActionDto> Actions = [];
|
||||
foreach (var action in StateMsg.ActionStates)
|
||||
{
|
||||
Actions.Add(new RobotActionDto()
|
||||
{
|
||||
ActionId = action.ActionId,
|
||||
Action = action,
|
||||
IsError = ActionFailed(action.ActionStatus),
|
||||
IsCompleted = ActionFinish(action.ActionStatus),
|
||||
IsProcessing = ActionRunning(action.ActionStatus),
|
||||
Errors = [.. GetErrorString()],
|
||||
});
|
||||
}
|
||||
return [.. Actions];
|
||||
}
|
||||
|
||||
private ErrorLevel GetErrorLevel()
|
||||
{
|
||||
if (StateMsg.Errors is not null)
|
||||
{
|
||||
if (StateMsg.Errors.Any(error => error.ErrorLevel == ErrorLevel.FATAL.ToString())) return ErrorLevel.FATAL;
|
||||
if (StateMsg.Errors.Any(error => error.ErrorLevel == ErrorLevel.WARNING.ToString())) return ErrorLevel.WARNING;
|
||||
}
|
||||
return ErrorLevel.NONE;
|
||||
}
|
||||
|
||||
private static bool ActionFinish(string status) => status == ActionStatus.FINISHED.ToString();
|
||||
|
||||
private static bool ActionFailed(string status) => status == ActionStatus.FAILED.ToString();
|
||||
|
||||
private static bool ActionRunning(string status) => status == ActionStatus.RUNNING.ToString();
|
||||
|
||||
private IEnumerable<string> GetErrorString()
|
||||
{
|
||||
if (StateMsg.Errors is not null && StateMsg.Errors.Length > 0)
|
||||
{
|
||||
foreach (var error in StateMsg.Errors)
|
||||
{
|
||||
yield return $"{error.ErrorType} - {error.ErrorDescription}";
|
||||
}
|
||||
}
|
||||
yield break;
|
||||
}
|
||||
|
||||
public Task<MessageResult> MoveRandom(List<string> nodes)
|
||||
{
|
||||
try
|
||||
{
|
||||
CancelRandom = new CancellationTokenSource();
|
||||
var random = new Random();
|
||||
var randomTask = Task.Run(async () =>
|
||||
{
|
||||
while (!CancelRandom.IsCancellationRequested)
|
||||
{
|
||||
var index = random.Next(nodes.Count);
|
||||
await MoveToNode(nodes[index]);
|
||||
while (!CancelRandom.IsCancellationRequested)
|
||||
{
|
||||
if (!IsWorking) break;
|
||||
await Task.Delay(1000);
|
||||
}
|
||||
await Task.Delay(2000);
|
||||
}
|
||||
}, cancellationToken: CancelRandom.Token);
|
||||
}
|
||||
catch { }
|
||||
return Task.FromResult<MessageResult>(new(true));
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
if (IsOnline) IsOnline = false;
|
||||
GC.SuppressFinalize(this);
|
||||
}
|
||||
}
|
||||
593
RobotNet.RobotManager/Services/Robot/RobotOrder.cs
Normal file
593
RobotNet.RobotManager/Services/Robot/RobotOrder.cs
Normal file
@@ -0,0 +1,593 @@
|
||||
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<ZoneDto> CurrentZones { get; set; } = [];
|
||||
|
||||
private const int intervalTime = 50;
|
||||
private readonly WatchTimerAsync<RobotOrder> Timer;
|
||||
private int TimerCounter = 0;
|
||||
|
||||
private OrderMsg OrderMsg;
|
||||
private readonly LoggerController<RobotOrder> Logger;
|
||||
private readonly TrafficManager TrafficManager;
|
||||
private readonly TrafficACS TrafficACS;
|
||||
private readonly MapManager MapManager;
|
||||
private readonly Func<OrderMsg, Task<MessageResult>> PubOrder;
|
||||
private readonly List<NodeDto> Nodes;
|
||||
private readonly List<EdgeDto> Edges;
|
||||
private readonly Dictionary<Guid, ZoneDto[]> Zones;
|
||||
private readonly IDictionary<string, IEnumerable<Action>> 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<Guid, ZoneDto[]> zones,
|
||||
IDictionary<string, IEnumerable<Action>> actions,
|
||||
OrderMsg orderMsg,
|
||||
IRobotController robotController,
|
||||
NavigationType navigationType,
|
||||
TrafficManager traffiManager,
|
||||
TrafficACS trafficACS,
|
||||
MapManager mapManager,
|
||||
LoggerController<RobotOrder> logger,
|
||||
Func<OrderMsg, Task<MessageResult>> 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<bool> 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<NodeDto> 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<EdgeDto> 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<NodeDto> 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<EdgeDto> 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<Guid> RequestInACS(Dictionary<Guid, ZoneDto[]> 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<string> 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<bool> 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<NavigationPathEdge> 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<NavigationPathEdge> 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<NavigationPathEdge> 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);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user