594 lines
25 KiB
C#
594 lines
25 KiB
C#
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);
|
|
}
|
|
}
|