RobotNet/RobotNet.RobotManager/Services/Traffic/TrafficManager.cs
2025-10-15 15:15:53 +07:00

857 lines
47 KiB
C#

using RobotNet.RobotManager.Data;
using RobotNet.RobotShares.Dtos;
using RobotNet.RobotShares.Enums;
using RobotNet.Shares;
using System.Collections.Concurrent;
namespace RobotNet.RobotManager.Services.Traffic;
public class TrafficManager(MapManager Map, IServiceProvider ServiceProvider, PathPlanner PathPlannger, IConfiguration Configuration, LoggerController<TrafficManager> Logger) : IHostedService
{
private System.Threading.Timer? computeTimer;
private const int intervalTime = 1000;
private readonly double TrafficCheckingDistanceMax = Configuration.GetValue("TrafficConfig:CheckingDistanceMax", 5);
private readonly double TrafficCheckingDistanceMin = Configuration.GetValue("TrafficConfig:CheckingDistanceMin", 3);
private readonly int TrafficResolutionRepeat = Configuration.GetValue("TrafficConfig:ResolutionRepeat", 1);
private readonly int TrafficAvoidableNodeMax = Configuration.GetValue("TrafficConfig:AvoidableNodeMax", 3);
private readonly double TrafficOffsetLength = Configuration.GetValue("TrafficConfig:OffsetLength", 0.3);
private readonly double TrafficOffsetWidth = Configuration.GetValue("TrafficConfig:OffsetWidth", 0.2);
public bool Enable => Configuration.GetValue("TrafficConfig:Enable", false);
private readonly SemaphoreSlim AgentSemaphore = new(1, 1);
private readonly SemaphoreSlim UpdateSemaphore = new(1, 1);
public ConcurrentDictionary<Guid, TrafficMap> TrafficMaps { get; set; } = [];
private Agent NewAgent(Guid mapId, IRobotController robotController, AgentModel model, TrafficNodeDto[] nodes, TrafficEdgeDto[] edges)
{
model.TurningRadius = TrafficMath.CalTurningRadius(model, TrafficOffsetLength, TrafficOffsetWidth);
foreach (var node in nodes)
{
node.AvoidablePaths = [.. Map.GetNegativePaths(mapId, new() { Id = node.Id, X = node.X, Y = node.Y, Name = node.Name }, model.TurningRadius)
.Where(path => path.Length > 0)
.Select(path => path.Select(n => new TrafficNodeDto { Id = n.Id, X = n.X, Y = n.Y, Name = n.Name }).ToArray())];
node.IsAvoidableNode = Map.GetNegativeNodes(mapId, node.Id).Length > 2 && node.AvoidablePaths.Length > 0;
if (nodes.Length > 0 && node != nodes.Last() && model.NavigationType != NavigationType.Forklift) node.LockedShapes = new()
{
Type = TrafficLockedShapeType.Circle,
Radius = model.TurningRadius,
};
else node.LockedShapes = TrafficMath.CalRobotRectShape(model, TrafficOffsetLength, TrafficOffsetWidth, node.X, node.Y, node.Theta);
}
Agent agent = new()
{
Robot = robotController,
AgentModel = model,
Nodes = [.. nodes],
InNodeIndex = 0,
ReleaseNode = nodes[0],
Edges = [.. edges],
State = TrafficSolutionState.Complete,
MapId = mapId,
GoalNode = nodes[^1],
};
return agent;
}
public MessageResult CreateAgent(Guid mapId, IRobotController robotController, AgentModel model, TrafficNodeDto[] nodes, TrafficEdgeDto[] edges)
{
if (AgentSemaphore.Wait(2000))
{
try
{
if (TrafficMaps.TryGetValue(mapId, out TrafficMap? TrafficMap) && TrafficMap is not null)
{
if (TrafficMap.Agents.TryGetValue(robotController.SerialNumber, out _)) TrafficMap.Agents.Remove(robotController.SerialNumber);
TrafficMap.Agents.Add(robotController.SerialNumber, NewAgent(mapId, robotController, model, nodes, edges));
}
else
{
bool result = TrafficMaps.TryAdd(mapId, new TrafficMap()
{
MapId = mapId,
Agents = new Dictionary<string, Agent>()
{
{
robotController.SerialNumber,
NewAgent(mapId, robotController, model, nodes, edges)
}
},
});
if (!result) return new(false, $"Không thể tạo TrafficMap cho bản đồ {mapId} do đã tồn tại hoặc xảy ra lỗi khi thêm agent {robotController.SerialNumber}.");
}
//Logger.Info($"CreateAgent: Tạo Agent thành công - {robotController.SerialNumber} to {nodes[^1].Name}, nodes: [{string.Join(", ", nodes.Select(n => $"[({n.Name}) - ({n.Direction})]"))}]");
return new(true, "");
}
catch (Exception ex)
{
Logger.Warning($"Tạo mới traffic Agent xảy ra lỗi - {ex.Message}");
return new(false, $"Tạo mới traffic Agent xảy ra lỗi - {ex.Message}");
}
finally
{
AgentSemaphore.Release();
}
}
else return new(false, $"Không thể tạo trafficAgent do vượt quá thời gian chờ.");
}
public MessageResult DeleteAgent(string robotId)
{
if (AgentSemaphore.Wait(2000))
{
try
{
using var scope = ServiceProvider.CreateScope();
var RobotDb = scope.ServiceProvider.GetRequiredService<RobotEditorDbContext>();
var robot = RobotDb.Robots.FirstOrDefault(r => r.RobotId == robotId);
if (robot is null) return new(false, $"Không tìm thấy robot Id {robotId} trong kho dữ liệu.");
if (TrafficMaps.TryGetValue(robot.MapId, out TrafficMap? TrafficMap) && TrafficMap is not null)
{
if (TrafficMap.Agents.TryGetValue(robotId, out _))
{
var remove = TrafficMap.Agents.Remove(robotId);
if (!remove) return new(false, $"Không thể xóa agent {robotId}.");
}
}
//Logger.Info($"DeleteAgent: Xóa Agent thành công - {robotId}");
return new(true, "");
}
catch (Exception ex)
{
Logger.Warning($"Xóa traffic Agent xảy ra lỗi - {ex.Message}");
return new(false, $"Xóa traffic Agent xảy ra lỗi - {ex.Message}");
}
finally
{
AgentSemaphore.Release();
}
}
else return new(false, $"Không thể xóa trafficAgent do vượt quá thời gian chờ.");
}
public TrafficSolution GetTrafficNode(string robotId)
{
try
{
using var scope = ServiceProvider.CreateScope();
var RobotDb = scope.ServiceProvider.GetRequiredService<RobotEditorDbContext>();
var robot = RobotDb.Robots.FirstOrDefault(r => r.RobotId == robotId);
if (robot is null) return new() { State = TrafficSolutionState.None };
if (TrafficMaps.TryGetValue(robot.MapId, out TrafficMap? trafficmap) && trafficmap is not null)
{
if (trafficmap.Agents.TryGetValue(robotId, out Agent? agent) && agent is not null)
{
TrafficSolution returnSolution = new()
{
State = agent.State,
ReleaseNode = agent.ReleaseNode,
GivewayEdges = agent.GivewayEdges,
GivewayNodes = agent.GivewayNodes,
Nodes = [.. agent.Nodes],
Edges = [.. agent.Edges]
}; ;
switch (agent.State)
{
case TrafficSolutionState.GiveWay:
if (agent.InNodeIndex != agent.ReleaseNodeIndex && (agent.GivewayNodes.Count > 0 && agent.ReleaseNode.Id != agent.GivewayNodes[^1].Id)) returnSolution.State = TrafficSolutionState.Waitting;
else
{
if (agent.GivewayNodes.Count > 0 && agent.ReleaseNode.Id != agent.GivewayNodes[^1].Id) agent.ReleaseNode = agent.GivewayNodes[^1];
if (agent.GivewayNodes.Count == 0 && agent.RefreshPathState == RefreshPathState.Compeleted)
{
agent.State = TrafficSolutionState.RefreshPath;
returnSolution.State = TrafficSolutionState.RefreshPath;
}
}
break;
case TrafficSolutionState.LoopResolve: returnSolution.State = TrafficSolutionState.Complete; break;
};
// Console.WriteLine($"{robotId} Gettraffic: {returnSolution.State}, release: {returnSolution.ReleaseNode.Name}");
return returnSolution;
}
}
return new() { State = TrafficSolutionState.None };
}
catch (Exception ex)
{
Logger.Warning($"{robotId} Lấy thông tin traffic xảy ra lỗi: {ex.Message}");
return new() { State = TrafficSolutionState.None };
}
}
public MessageResult UpdateInNode(string robotId, Guid nodeId)
{
if (UpdateSemaphore.Wait(2000))
{
try
{
using var scope = ServiceProvider.CreateScope();
var RobotDb = scope.ServiceProvider.GetRequiredService<RobotEditorDbContext>();
var robot = RobotDb.Robots.FirstOrDefault(r => r.RobotId == robotId);
if (robot is null) return new(false, $"Không tìm thấy robot Id {robotId} trong kho dữ liệu.");
if (TrafficMaps.TryGetValue(robot.MapId, out TrafficMap? trafficmap) && trafficmap is not null)
{
if (trafficmap.Agents.TryGetValue(robotId, out Agent? agent) && agent is not null)
{
if (agent.State == TrafficSolutionState.GiveWay)
{
if (agent.GivewayNodes.Count > 1 && nodeId == agent.GivewayNodes[^1].Id)
{
var giveWayResolution = trafficmap.GivewayResolution.FirstOrDefault(n => n.RobotGive == robotId);
if (giveWayResolution is not null)
{
trafficmap.AddLocker(giveWayResolution.RobotReceive, [.. giveWayResolution.Nodes]);
giveWayResolution.IsGiveway = true;
}
agent.Nodes = agent.SubNodes;
agent.Edges = agent.SubEdges;
agent.GivewayNodes = [];
agent.GivewayEdges = [];
}
List<TrafficNodeDto> lockedNodes = [];
var nodeIndex = agent.Nodes.FindIndex(n => n.Id == nodeId);
if (nodeIndex != -1)
{
agent.InNodeIndex = nodeIndex;
if (agent.ReleaseNodeIndex != -1 && agent.InNodeIndex <= agent.ReleaseNodeIndex)
lockedNodes = agent.Nodes.GetRange(agent.InNodeIndex, agent.ReleaseNodeIndex - agent.InNodeIndex + 1);
lockedNodes.AddRange(agent.GivewayNodes.Where(n => !lockedNodes.Any(ln => ln.Id == n.Id)));
}
else
{
nodeIndex = agent.GivewayNodes.FindIndex(n => n.Id == nodeId);
if (nodeIndex != -1) lockedNodes = agent.GivewayNodes.GetRange(nodeIndex, agent.GivewayNodes.Count - nodeIndex);
}
trafficmap.UpdateLocker(robotId, [.. lockedNodes]);
}
else
{
var nodeIndex = agent.Nodes.FindIndex(n => n.Id == nodeId);
if (nodeIndex != -1)
{
agent.InNodeIndex = nodeIndex;
if (agent.ReleaseNodeIndex != -1 && agent.InNodeIndex <= agent.ReleaseNodeIndex)
{
var lockedNodes = agent.Nodes.GetRange(agent.InNodeIndex, agent.ReleaseNodeIndex - agent.InNodeIndex + 1);
trafficmap.UpdateLocker(robotId, [.. lockedNodes]);
trafficmap.GivewayResolution.RemoveAll(s => s.RobotReceive == agent.Robot.SerialNumber && s.IsGiveway && !lockedNodes.Any(n => s.Nodes.Any(sn => sn.Id == n.Id)));
}
if (agent.State == TrafficSolutionState.LoopResolve && nodeId == agent.ReleaseNode.Id)
{
var giveWayResolution = trafficmap.GivewayResolution.FirstOrDefault(n => n.RobotGive == robotId);
if (giveWayResolution is not null)
{
trafficmap.AddLocker(giveWayResolution.RobotReceive, [.. giveWayResolution.Nodes]);
giveWayResolution.IsGiveway = true;
}
agent.State = TrafficSolutionState.Waitting;
}
}
}
}
}
return new(true, "");
}
catch (Exception ex)
{
Logger.Warning($"{robotId} Cập nhật vị trí xảy ra lỗi: {ex.Message}");
return new(false, $"{robotId} Cập nhật vị trí xảy ra lỗi: {ex.Message}");
}
finally
{
UpdateSemaphore.Release();
}
}
else return new(false, $"Không thể xóa cập nhật dữ liệu vào hệ thống traffic do vượt quá thời gian chờ.");
}
private static (TrafficConflictState state, string robotId, TrafficNodeDto? nodeId) FirstCheckingReleaseNodes(TrafficNodeDto[] releaseNodes, Agent agent, TrafficMap trafficMap)
{
if (releaseNodes.Length != 0)
{
foreach (var releaseNode in releaseNodes)
{
foreach (var locked in trafficMap.Locked)
{
if (locked.Key == agent.Robot.SerialNumber) continue;
if (locked.Value.Any(n => n.Id == releaseNode.Id)) return (TrafficConflictState.Vertex, locked.Key, releaseNode);
//foreach (var lockedNode in locked.Value)
//{
// var distance = Math.Sqrt(Math.Pow(lockedNode.X - releaseNode.X, 2) + Math.Pow(lockedNode.Y - releaseNode.Y, 2));
// if(distance > (lockedNode.LockedShapes.Radius + releaseNode.LockedShapes.Radius)) continue;
// if (lockedNode.LockedShapes.Type == TrafficLockedShapeType.Circle)
// {
// if (releaseNode.LockedShapes.Type == TrafficLockedShapeType.Circle)
// {
// if (distance < (lockedNode.LockedShapes.Radius + releaseNode.LockedShapes.Radius)) return (TrafficConflictState.Proximity, locked.Key, releaseNode);
// }
// else if (releaseNode.LockedShapes.Type == TrafficLockedShapeType.Rectangle)
// {
// if(TrafficMath.RectIntersectCircle(lockedNode, lockedNode.LockedShapes.Radius,
// releaseNode.LockedShapes.X1, releaseNode.LockedShapes.Y1,
// releaseNode.LockedShapes.X2, releaseNode.LockedShapes.Y2,
// releaseNode.LockedShapes.X3, releaseNode.LockedShapes.Y3,
// releaseNode.LockedShapes.X4, releaseNode.LockedShapes.Y4)) return (TrafficConflictState.Proximity, locked.Key, releaseNode);
// }
// }
// else if (lockedNode.LockedShapes.Type == TrafficLockedShapeType.Rectangle)
// {
// if (releaseNode.LockedShapes.Type == TrafficLockedShapeType.Circle)
// {
// if(TrafficMath.RectIntersectRect(lockedNode.LockedShapes, releaseNode.LockedShapes)) return (TrafficConflictState.Proximity, locked.Key, releaseNode);
// }
// else if (releaseNode.LockedShapes.Type == TrafficLockedShapeType.Circle)
// {
// if (TrafficMath.RectIntersectCircle(releaseNode, releaseNode.LockedShapes.Radius,
// lockedNode.LockedShapes.X1, lockedNode.LockedShapes.Y1,
// lockedNode.LockedShapes.X2, lockedNode.LockedShapes.Y2,
// lockedNode.LockedShapes.X3, lockedNode.LockedShapes.Y3,
// lockedNode.LockedShapes.X4, lockedNode.LockedShapes.Y4)) return (TrafficConflictState.Proximity, locked.Key, releaseNode);
// }
// }
//}
}
}
}
return (TrafficConflictState.None, string.Empty, null);
}
private static (bool IsSuccess, string robotId, TrafficNodeDto? nodeId) GivewayCheckingReleaseNodes(TrafficNodeDto[] releaseNodes, string robotId, TrafficMap trafficMap)
{
foreach (var node in releaseNodes)
{
foreach (var locked in trafficMap.Locked)
{
if (locked.Value.Any(n => (n.Id == node.Id)) && locked.Key != robotId)
return (false, locked.Key, node);
}
}
return (true, string.Empty, null);
}
private void CheckConflict(Agent agent, TrafficMap trafficMap)
{
if (agent.Checking(TrafficAvoidableNodeMax, TrafficCheckingDistanceMax) && !trafficMap.GivewayResolution.Any(s => s.RobotGive == agent.Robot.SerialNumber))
{
var releaseNodes = agent.GetChekingNodes(TrafficAvoidableNodeMax, TrafficCheckingDistanceMin);
if (releaseNodes.Length > 0)
{
(var state, string conflictRobotId, TrafficNodeDto? conflictNode) = FirstCheckingReleaseNodes(releaseNodes, agent, trafficMap);
if (state == TrafficConflictState.None)
{
trafficMap.AddLocker(agent.Robot.SerialNumber, [.. releaseNodes]);
agent.ReleaseNode = releaseNodes[^1];
agent.State = TrafficSolutionState.Complete;
trafficMap.Conflicts.RemoveAll(conflict => conflict.AgentRequest.Robot.SerialNumber == agent.Robot.SerialNumber);
}
else
{
if (conflictNode is null)
{
Logger.Warning($"{agent.Robot.SerialNumber} kiểm tra xung đột có lỗi: xảy ra xung đột nhưng không tìm thấy node xung đột");
return;
}
if (trafficMap.Agents.TryGetValue(conflictRobotId, out Agent? conflictAgent) && conflictAgent is not null)
{
trafficMap.Conflicts.RemoveAll(c => c.AgentRequest.Robot.SerialNumber == agent.Robot.SerialNumber);
var conflictState = GetConflictState(agent, conflictAgent, conflictNode);
if (conflictState == TrafficConflictState.Confrontation || state == TrafficConflictState.Proximity)
{
Level1ConflictResolution(agent, conflictAgent, [.. releaseNodes], conflictNode, trafficMap);
}
else
{
trafficMap.Conflicts.Add(new()
{
AgentConflict = conflictAgent,
AgentRequest = agent,
NodeConflict = conflictNode,
ReleaseNodes = [.. releaseNodes],
State = TrafficConflictState.Vertex,
});
agent.State = TrafficSolutionState.Waitting;
}
}
else
{
// cần xử lí khi bị xung đột với robot đang không di chuyển
}
}
}
}
}
private static TrafficConflictState GetConflictState(Agent agent1, Agent agent2, TrafficNodeDto conflictNode)
{
var conflictNodeAgent2Index = agent2.Nodes.IndexOf(conflictNode);
if (conflictNodeAgent2Index != -1)
{
var remainingNodes = agent2.Nodes.GetRange(conflictNodeAgent2Index, agent2.Nodes.Count - conflictNodeAgent2Index);
if (remainingNodes.Any(n => n.Id == agent1.ReleaseNode.Id)) return TrafficConflictState.Confrontation;
return TrafficConflictState.Vertex;
}
return TrafficConflictState.None;
}
private TrafficSolutionState Level1ConflictResolution(Agent agent1, Agent agent2, List<TrafficNodeDto> releaseNodes, TrafficNodeDto conflictNode, TrafficMap trafficMap)
{
try
{
var remainingNodes = agent2.Nodes.GetRange(agent2.InNodeIndex, agent2.Nodes.Count - agent2.InNodeIndex);
var conflictIndex = releaseNodes.IndexOf(conflictNode);
if (conflictIndex != -1)
{
//double distance = 0;
for (int i = conflictIndex - 1; i >= 0; i--)
{
//distance += Math.Sqrt(Math.Pow(releaseNodes[i].X - releaseNodes[i + 1].X, 2) + Math.Pow(releaseNodes[i].Y - releaseNodes[i + 1].Y, 2));
//if (distance < TrafficLockedCircle) continue;
if (!remainingNodes.Any(n => n.Id == releaseNodes[i].Id))
{
var givewayNodes = releaseNodes.GetRange(0, releaseNodes.Count - i);
(var IsSuccess, string conflictRobotId, TrafficNodeDto? _) = GivewayCheckingReleaseNodes([.. givewayNodes], agent1.Robot.SerialNumber, trafficMap);
if (IsSuccess)
{
trafficMap.AddLocker(agent1.Robot.SerialNumber, [.. givewayNodes]);
agent1.State = TrafficSolutionState.Complete;
agent1.ReleaseNode = givewayNodes[^1];
trafficMap.Conflicts.RemoveAll(conflict => conflict.AgentRequest.Robot.SerialNumber == agent1.Robot.SerialNumber);
return TrafficSolutionState.Complete;
}
}
if (!releaseNodes[i].IsAvoidableNode) continue;
var state = Level2ConflictResolution(agent1, agent2, [.. releaseNodes[i].AvoidablePaths], [.. releaseNodes], [.. releaseNodes.GetRange(0, i + 1)], trafficMap);
if (state != TrafficSolutionState.UnableResolve) return state;
}
if (!trafficMap.GivewayResolution.Any(s => s.RobotReceive == agent1.Robot.SerialNumber))
{
trafficMap.Conflicts.Add(new()
{
AgentRequest = agent1,
AgentConflict = agent2,
NodeConflict = conflictNode,
ReleaseNodes = releaseNodes,
State = TrafficConflictState.Confrontation,
});
}
return TrafficSolutionState.UnableResolve;
}
else
{
Logger.Warning($"{agent1.Robot.SerialNumber} tìm kiếm giải pháp tránh xung đột bậc 1 xảy ra lỗi: confilct node không được tìm thấy trong release node {conflictNode.Name}, release list [{string.Join(", ", releaseNodes.Select(n => n.Name))}]");
}
return TrafficSolutionState.UnableResolve;
}
catch (Exception ex)
{
Logger.Warning($"{agent1.Robot.SerialNumber} tìm kiếm giải pháp tránh xung đột bậc 1 xảy ra lỗi: {ex.Message}");
return TrafficSolutionState.UnableResolve;
}
}
private TrafficSolutionState Level2ConflictResolution(Agent agent1, Agent agent2, TrafficNodeDto[][] avoidablePaths, TrafficNodeDto[] relaseNodes, TrafficNodeDto[] conflictNodes, TrafficMap trafficMap)
{
try
{
if (agent2.State == TrafficSolutionState.GiveWay) return TrafficSolutionState.Waitting;
List<(Agent conflictAgent, TrafficNodeDto conflictNode)> bufferConflict = [];
var remainingNodeAgent2 = agent2.Nodes.GetRange(agent2.InNodeIndex, agent2.Nodes.Count - agent2.InNodeIndex);
var remainingReleaseNodeAgent2 = agent2.Nodes.GetRange(agent2.InNodeIndex, agent2.ReleaseNodeIndex - agent2.InNodeIndex + 1);
foreach (var avoidablePath in avoidablePaths)
{
if (avoidablePath.Any(avoidableNode => remainingReleaseNodeAgent2.Any(n => n.Id == avoidableNode.Id)) ||
remainingNodeAgent2.Any(avoidableNode => avoidableNode.Id == avoidablePath[^1].Id)) continue;
(var IsSuccess, string conflictRobotId, TrafficNodeDto? conflictNode) = GivewayCheckingReleaseNodes([.. conflictNodes, .. avoidablePath], agent1.Robot.SerialNumber, trafficMap);
if (IsSuccess)
{
TrafficNodeDto[] givewayNodes = [.. conflictNodes, .. avoidablePath.Skip(1)];
trafficMap.AddLocker(agent1.Robot.SerialNumber, givewayNodes);
var (state, message) = agent1.UpdateGiveWay(givewayNodes, PathPlannger, Map);
Logger.Info($"{agent1.Robot.SerialNumber} tránh Robot {agent2.Robot.SerialNumber} tại Node {givewayNodes[^1].Name} {(state == TrafficSolutionState.Complete || state == TrafficSolutionState.GiveWay ? "thành công" : $"xảy ra lỗi: {message}")}");
if (state == TrafficSolutionState.GiveWay)
trafficMap.GivewayResolution.Add(new()
{
RobotGive = agent1.Robot.SerialNumber,
RobotReceive = agent2.Robot.SerialNumber,
Nodes = [conflictNodes[^1]]
});
return state;
}
else
{
if (conflictNode is null)
{
Logger.Warning($"{agent1.Robot.SerialNumber} tìm kiếm giải pháp tránh xung đột bậc 2 có lỗi: xảy ra xung đột nhưng không tìm thấy node xung đột");
}
else
{
if (trafficMap.Agents.TryGetValue(conflictRobotId, out Agent? conflictAgent) && conflictAgent is not null)
bufferConflict.Add((conflictAgent, conflictNode));
}
}
}
foreach (var (conflictAgent, conflictNode) in bufferConflict)
{
var trafficState = Level3ConflictResolution(conflictAgent, agent1.Robot.SerialNumber, conflictNode, relaseNodes, trafficMap, 0);
if (trafficState == TrafficSolutionState.Complete || trafficState == TrafficSolutionState.GiveWay) return trafficState;
}
//return FindAvoidableResolution(agent1, agent2, trafficMap) ;
return TrafficSolutionState.UnableResolve;
}
catch (Exception ex)
{
Logger.Warning($"{agent1.Robot.SerialNumber} tìm kiếm giải pháp tránh xung đột bậc 2 xảy ra lỗi: {ex.Message}");
return TrafficSolutionState.UnableResolve;
}
}
private TrafficSolutionState Level3ConflictResolution(Agent agent, string giveWayRobotId, TrafficNodeDto giveNode, TrafficNodeDto[] relaseNodes, TrafficMap trafficMap, int counter)
{
try
{
if (agent.State == TrafficSolutionState.GiveWay || agent.State == TrafficSolutionState.LoopResolve) return TrafficSolutionState.Waitting;
List<(Agent conflictAgent, TrafficNodeDto conflictNode)> bufferConflict = [];
var negativePaths = Map.GetNegativePaths(trafficMap.MapId, new() { Id = giveNode.Id, X = giveNode.X, Y = giveNode.Y, Name = giveNode.Name }, agent.AgentModel.TurningRadius)
.Where(path => path.Length > 0)
.Select(path => path.Select(n => new TrafficNodeDto { Id = n.Id, X = n.X, Y = n.Y, Name = n.Name }).ToArray())
.ToList();
foreach (var negativePath in negativePaths)
{
if (negativePath.Any(negativeNode => relaseNodes.Any(n => n.Id == negativeNode.Id) && negativeNode.Id != giveNode.Id)) continue;
(var IsSuccess, string conflictRobotId, TrafficNodeDto? conflictNode) = GivewayCheckingReleaseNodes(negativePath, agent.Robot.SerialNumber, trafficMap);
if (IsSuccess)
{
trafficMap.AddLocker(agent.Robot.SerialNumber, negativePath);
var (state, message) = agent.UpdateGiveWay(negativePath, PathPlannger, Map);
Logger.Info($"{agent.Robot.SerialNumber} tránh level3 Robot {giveWayRobotId} tại Node {negativePath[^1].Name} {(state == TrafficSolutionState.Complete || state == TrafficSolutionState.GiveWay ? "thành công" : $"xảy ra lỗi: {message}")}");
if (state == TrafficSolutionState.GiveWay)
trafficMap.GivewayResolution.Add(new()
{
RobotGive = agent.Robot.SerialNumber,
RobotReceive = giveWayRobotId,
Nodes = [giveNode]
});
return state;
}
else
{
if (conflictNode is null)
{
Logger.Warning($"{agent.Robot.SerialNumber} tìm kiếm giải pháp tránh xung đột bậc 3 có lỗi: xảy ra xung đột nhưng không tìm thấy node xung đột");
}
else
{
if (trafficMap.Agents.TryGetValue(conflictRobotId, out Agent? conflictAgent) && conflictAgent is not null)
bufferConflict.Add((conflictAgent, conflictNode));
}
}
}
if (counter < TrafficResolutionRepeat)
{
foreach (var (conflictAgent, conflictNode) in bufferConflict)
{
var trafficState = Level3ConflictResolution(conflictAgent, agent.Robot.SerialNumber, conflictNode, [.. relaseNodes, giveNode], trafficMap, ++counter);
if (trafficState == TrafficSolutionState.Complete || trafficState == TrafficSolutionState.GiveWay) return trafficState;
}
}
return TrafficSolutionState.UnableResolve;
}
catch (Exception ex)
{
Logger.Warning($"{agent.Robot.SerialNumber} tìm kiếm giải pháp tránh xung đột bậc 3 xảy ra lỗi: {ex.Message}");
return TrafficSolutionState.UnableResolve;
}
}
private TrafficSolutionState FindAvoidableResolution(Agent agent1, Agent agent2, TrafficMap trafficMap)
{
try
{
if (agent2.State == TrafficSolutionState.GiveWay || agent2.State == TrafficSolutionState.LoopResolve) return TrafficSolutionState.Waitting;
if (agent2.ReleaseNodeIndex == -1 || agent1.ReleaseNodeIndex == -1) return TrafficSolutionState.Waitting;
var conflictNodes = agent2.Nodes.GetRange(agent2.ReleaseNodeIndex, agent2.Nodes.Count - agent2.ReleaseNodeIndex);
List<(Agent conflictAgent, TrafficNodeDto conflictNode)> bufferConflict = [];
for (int i = 1; i < conflictNodes.Count; i++)
{
if (conflictNodes[i].IsAvoidableNode)
{
var remainingNodeAgent2 = agent2.Nodes.GetRange(agent2.ReleaseNodeIndex, agent2.Nodes.Count - agent2.ReleaseNodeIndex);
foreach (var avoidablePath in conflictNodes[i].AvoidablePaths)
{
if (remainingNodeAgent2.Any(n => n.Id == avoidablePath[^1].Id)) continue;
var givewayagent2 = conflictNodes.GetRange(1, i);
var mergeNode = givewayagent2.LastOrDefault(n => agent1.Nodes.Any(a1 => a1.Id == n.Id));
if (mergeNode is not null)
{
var avoidableIndex = givewayagent2.FindIndex(n => n.Id == mergeNode.Id);
if (avoidableIndex != -1)
{
var giveWayNodes = givewayagent2.GetRange(avoidableIndex, givewayagent2.Count - avoidableIndex - 1);
giveWayNodes.AddRange(avoidablePath);
(var IsSuccess, string conflictRobotId, TrafficNodeDto? conflictNode) = GivewayCheckingReleaseNodes([.. giveWayNodes], agent1.Robot.SerialNumber, trafficMap);
if (IsSuccess)
{
trafficMap.AddLocker(agent1.Robot.SerialNumber, [.. giveWayNodes]);
var (state, message) = agent1.UpdateGiveWay([.. giveWayNodes], PathPlannger, Map);
Logger.Info($"{agent1.Robot.SerialNumber} giải quyết đối đầu với robot {agent2.Robot.SerialNumber} tại node {giveWayNodes[^1].Name} {(state == TrafficSolutionState.Complete || state == TrafficSolutionState.GiveWay ? "thành công" : $"xảy ra lỗi: {message}")}");
if (state == TrafficSolutionState.GiveWay)
trafficMap.GivewayResolution.Add(new()
{
RobotGive = agent1.Robot.SerialNumber,
RobotReceive = agent2.Robot.SerialNumber,
Nodes = [conflictNodes[i]]
});
return state;
}
else
{
if (conflictNode is null)
{
Logger.Warning($"{agent1.Robot.SerialNumber} tìm kiếm giải pháp tránh xung đột đối đầu có lỗi: xảy ra xung đột nhưng không tìm thấy node xung đột");
}
else
{
if (trafficMap.Agents.TryGetValue(conflictRobotId, out Agent? conflictAgent) && conflictAgent is not null)
bufferConflict.Add((conflictAgent, conflictNode));
}
}
}
}
}
}
}
foreach (var (conflictAgent, conflictNode) in bufferConflict)
{
var trafficState = Level3ConflictResolution(conflictAgent, agent1.Robot.SerialNumber, conflictNode, [.. conflictNodes], trafficMap, 0);
if (trafficState == TrafficSolutionState.Complete || trafficState == TrafficSolutionState.GiveWay) return trafficState;
}
return TrafficSolutionState.UnableResolve;
}
catch (Exception ex)
{
Logger.Warning($"{agent1.Robot.SerialNumber} tìm kiếm giải pháp tránh xung đột đối đầu xảy ra lỗi: {ex.Message}");
return TrafficSolutionState.UnableResolve;
}
}
private TrafficSolutionState LoopConflictResolution(TrafficConflict[] conflicts, TrafficMap trafficMap)
{
try
{
for (int i = 0; i < conflicts.Length; i++)
{
var conflictAvoid = conflicts.FirstOrDefault(c => c.AgentConflict.Robot.SerialNumber == conflicts[i].AgentRequest.Robot.SerialNumber);
if (conflictAvoid is null) continue;
var conflictResolutionState = LoopConflictLevel1Resolution(conflicts[i], conflictAvoid, trafficMap);
if (conflictResolutionState == TrafficSolutionState.Complete) return conflictResolutionState;
}
for (int i = 0; i < conflicts.Length; i++)
{
var conflictAvoid = conflicts.FirstOrDefault(c => c.AgentConflict.Robot.SerialNumber == conflicts[i].AgentRequest.Robot.SerialNumber);
if (conflictAvoid is null) continue;
var conflictResolutionState = LoopConflictLevel2Resolution(conflicts[i], conflictAvoid, trafficMap);
if (conflictResolutionState == TrafficSolutionState.Complete) return conflictResolutionState;
}
return TrafficSolutionState.UnableResolve;
}
catch (Exception ex)
{
Logger.Warning($"LoopConflictResolution: Hệ thống xảy ra lỗi - {ex.Message}");
return TrafficSolutionState.UnableResolve;
}
}
private TrafficSolutionState LoopConflictLevel1Resolution(TrafficConflict conflict1, TrafficConflict conflict2, TrafficMap trafficMap)
{
try
{
var agent1ConflictIndex = conflict1.ReleaseNodes.IndexOf(conflict1.NodeConflict);
var agent2ConflictIndex = conflict2.ReleaseNodes.IndexOf(conflict2.NodeConflict);
if (agent1ConflictIndex == -1 || agent2ConflictIndex == -1) return TrafficSolutionState.UnableResolve;
var agent1ConflictNodes = conflict1.ReleaseNodes.GetRange(0, agent1ConflictIndex);
var agent2ConflictNodes = conflict2.ReleaseNodes.GetRange(0, agent2ConflictIndex + 1);
Logger.Info($"{conflict1.AgentRequest.Robot.SerialNumber} xung đột lặp lại với robot {conflict2.AgentRequest.Robot.SerialNumber} - release 1: [{conflict1.ReleaseNodes[0].Name} => {conflict1.ReleaseNodes[^1].Name} - {conflict1.NodeConflict.Name}] - release 2: [{conflict2.ReleaseNodes[0].Name} => {conflict2.ReleaseNodes[^1].Name} - {conflict2.NodeConflict.Name}]");
var avoidableNode = agent1ConflictNodes.FirstOrDefault(n => !agent2ConflictNodes.Any(n2 => n2.Id == n.Id));
if (avoidableNode is not null)
{
TrafficNodeDto[] releaseNodes = [.. conflict1.ReleaseNodes.GetRange(0, conflict1.ReleaseNodes.IndexOf(avoidableNode) + 1)];
(var conflictState, string conflictRobotId, TrafficNodeDto? conflictNode) = FirstCheckingReleaseNodes(releaseNodes, conflict1.AgentRequest, trafficMap);
if (conflictState == TrafficConflictState.None)
{
Logger.Info($"{conflict1.AgentRequest.Robot.SerialNumber} giải quyết xung đột lặp lại với robot {conflict2.AgentRequest.Robot.SerialNumber} tại node {avoidableNode.Name} - release {releaseNodes[^1].Name} ");
trafficMap.AddLocker(conflict1.AgentRequest.Robot.SerialNumber, [.. releaseNodes]);
conflict1.AgentRequest.ReleaseNode = releaseNodes[^1];
conflict1.AgentRequest.State = TrafficSolutionState.LoopResolve;
trafficMap.GivewayResolution.Add(new()
{
RobotGive = conflict1.AgentRequest.Robot.SerialNumber,
RobotReceive = conflict2.AgentRequest.Robot.SerialNumber,
Nodes = [.. agent2ConflictNodes]
});
return TrafficSolutionState.Complete;
}
}
return TrafficSolutionState.Waitting;
}
catch (Exception ex)
{
Logger.Warning($"LoopConflictLevel1Resolution: Hệ thống xảy ra lỗi - {ex.Message}");
return TrafficSolutionState.UnableResolve;
}
}
private TrafficSolutionState LoopConflictLevel2Resolution(TrafficConflict conflict1, TrafficConflict conflict2, TrafficMap trafficMap)
{
try
{
var agent2ConflictIndex = conflict2.ReleaseNodes.IndexOf(conflict2.NodeConflict);
if (agent2ConflictIndex == -1) return TrafficSolutionState.UnableResolve;
var agent2ConflictNodes = conflict2.ReleaseNodes.GetRange(0, agent2ConflictIndex + 1);
Logger.Info($"{conflict1.AgentRequest.Robot.SerialNumber} xung đột lặp lại với robot {conflict2.AgentRequest.Robot.SerialNumber} - release 1: [{conflict1.ReleaseNodes[0].Name} => {conflict1.ReleaseNodes[^1].Name} - {conflict1.NodeConflict.Name}] - release 2: [{conflict2.ReleaseNodes[0].Name} => {conflict2.ReleaseNodes[^1].Name} - {conflict2.NodeConflict.Name}]");
var avoidableNode = conflict1.ReleaseNodes.FirstOrDefault(n => !agent2ConflictNodes.Any(n2 => n2.Id == n.Id));
if (avoidableNode is not null)
{
TrafficNodeDto[] releaseNodes = [.. conflict1.ReleaseNodes.GetRange(0, conflict1.ReleaseNodes.IndexOf(avoidableNode) + 1)];
(var conflictState, string conflictRobotId, TrafficNodeDto? conflictNode) = FirstCheckingReleaseNodes(releaseNodes, conflict1.AgentRequest, trafficMap);
if (conflictState == TrafficConflictState.None)
{
Logger.Info($"{conflict1.AgentRequest.Robot.SerialNumber} giải quyết xung đột lặp lại với robot {conflict2.AgentRequest.Robot.SerialNumber} tại node {avoidableNode.Name} - release {releaseNodes[^1].Name} ");
trafficMap.AddLocker(conflict1.AgentRequest.Robot.SerialNumber, [.. releaseNodes]);
conflict1.AgentRequest.ReleaseNode = releaseNodes[^1];
conflict1.AgentRequest.State = TrafficSolutionState.LoopResolve;
trafficMap.GivewayResolution.Add(new()
{
RobotGive = conflict1.AgentRequest.Robot.SerialNumber,
RobotReceive = conflict2.AgentRequest.Robot.SerialNumber,
Nodes = [.. agent2ConflictNodes]
});
return TrafficSolutionState.Complete;
}
else
{
if (trafficMap.Agents.TryGetValue(conflictRobotId, out Agent? conflictAgent) && conflictAgent is not null)
{
var state = Level3ConflictResolution(conflictAgent, conflict1.AgentRequest.Robot.SerialNumber, avoidableNode, [.. releaseNodes, .. agent2ConflictNodes], trafficMap, 0);
if (state == TrafficSolutionState.Complete || state == TrafficSolutionState.GiveWay)
{
trafficMap.Conflicts.RemoveAll(c => c.AgentRequest.Robot.SerialNumber == conflictAgent.Robot.SerialNumber);
return TrafficSolutionState.Complete;
}
return TrafficSolutionState.UnableResolve;
}
}
}
return TrafficSolutionState.Waitting;
}
catch (Exception ex)
{
Logger.Warning($"LoopConflictLevel2Resolution: Hệ thống xảy ra lỗi - {ex.Message}");
return TrafficSolutionState.UnableResolve;
}
}
private void ComputeHandler(object? state)
{
try
{
foreach (var trafficmap in TrafficMaps)
{
var agents = trafficmap.Value.Agents.Values.ToList();
foreach (var agent in agents)
{
CheckConflict(agent, trafficmap.Value);
}
var conflicts = trafficmap.Value.Conflicts.ToList();
for (int i = 0; i < conflicts.Count; i++)
{
int repeat = 0;
bool isProccessed = false;
string agentConflict = conflicts[i].AgentConflict.Robot.SerialNumber;
List<TrafficConflict> loopConflicts = [conflicts[i]];
while (repeat++ <= TrafficResolutionRepeat)
{
var loopConflict = conflicts.FirstOrDefault(cl => cl.AgentRequest.Robot.SerialNumber == agentConflict);
if (loopConflict is null) break;
loopConflicts.Add(loopConflict);
if (loopConflict.AgentConflict.Robot.SerialNumber == conflicts[i].AgentRequest.Robot.SerialNumber)
{
if (loopConflicts.Count > 2)
{
var resolutionstate = LoopConflictResolution([.. loopConflicts], trafficmap.Value);
isProccessed = resolutionstate == TrafficSolutionState.Complete;
}
break;
}
else agentConflict = loopConflict.AgentConflict.Robot.SerialNumber;
}
if (isProccessed) break;
if (conflicts[i].State == TrafficConflictState.Confrontation)
{
if (conflicts.Any(c => c.State == TrafficConflictState.Confrontation && c.AgentRequest.Robot.SerialNumber == conflicts[i].AgentConflict.Robot.SerialNumber))
{
var resolutionstate = FindAvoidableResolution(conflicts[i].AgentRequest, conflicts[i].AgentConflict, trafficmap.Value);
if (resolutionstate == TrafficSolutionState.Complete || resolutionstate == TrafficSolutionState.GiveWay)
{
trafficmap.Value.Conflicts.Remove(conflicts[i]);
break;
}
}
}
}
}
computeTimer?.Change(intervalTime, 0);
}
catch (Exception ex)
{
Logger.Warning($"TrafficCompute: Hệ thống xảy ra lỗi - {ex.Message}");
computeTimer?.Change(intervalTime, 0);
}
}
public async Task StartAsync(CancellationToken cancellationToken)
{
if (!Enable) return;
computeTimer = new Timer(ComputeHandler, null, Timeout.Infinite, Timeout.Infinite);
computeTimer.Change(intervalTime, 0);
await Task.Yield();
}
public async Task StopAsync(CancellationToken cancellationToken)
{
if (computeTimer != null)
{
computeTimer.Change(Timeout.Infinite, Timeout.Infinite);
await computeTimer.DisposeAsync();
}
}
}