751 lines
33 KiB
C#
751 lines
33 KiB
C#
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);
|
|
}
|
|
}
|