270 lines
14 KiB
C#
270 lines
14 KiB
C#
using RobotNet.MapShares;
|
|
using RobotNet.MapShares.Dtos;
|
|
using RobotNet.RobotManager.Services.Planner.Space;
|
|
using RobotNet.RobotShares.Dtos;
|
|
using RobotNet.RobotShares.Enums;
|
|
|
|
namespace RobotNet.RobotManager.Services.Traffic;
|
|
|
|
|
|
public class Agent
|
|
{
|
|
public IRobotController Robot { get; set; } = null!;
|
|
public AgentModel AgentModel { get; set; } = new();
|
|
public Guid MapId { get; set; }
|
|
public int InNodeIndex { get; set; }
|
|
public TrafficNodeDto InNode { get; set; } = new();
|
|
public int ReleaseNodeIndex => Nodes.IndexOf(ReleaseNode);
|
|
public TrafficNodeDto ReleaseNode { get; set; } = new();
|
|
public TrafficNodeDto GoalNode { get; set; } = new();
|
|
public List<TrafficNodeDto> Nodes { get; set; } = [];
|
|
public List<TrafficEdgeDto> Edges { get; set; } = [];
|
|
public List<TrafficNodeDto> SubNodes { get; set; } = [];
|
|
public List<TrafficEdgeDto> SubEdges { get; set; } = [];
|
|
public List<TrafficNodeDto> GivewayNodes { get; set; } = [];
|
|
public List<TrafficEdgeDto> GivewayEdges { get; set; } = [];
|
|
public TrafficSolutionState State { get; set; }
|
|
public RefreshPathState RefreshPathState { get; set; } = RefreshPathState.Compeleted;
|
|
public string Message { get; set; } = "";
|
|
|
|
private static double GetDistance(List<TrafficNodeDto> nodes)
|
|
{
|
|
double distance = 0;
|
|
for (int i = 0; i < nodes.Count - 1; i++)
|
|
{
|
|
distance += Math.Sqrt(Math.Pow(nodes[i].X - nodes[i + 1].X, 2) + Math.Pow(nodes[i].Y - nodes[i + 1].Y, 2));
|
|
}
|
|
return distance;
|
|
}
|
|
|
|
public bool Checking(double trafficAvoidableNodeMax, double trafficDistanceMax)
|
|
{
|
|
if (State != TrafficSolutionState.GiveWay && State != TrafficSolutionState.LoopResolve &&
|
|
RefreshPathState != RefreshPathState.Created && RefreshPathState != RefreshPathState.Refreshing)
|
|
{
|
|
if (ReleaseNodeIndex != -1 && InNodeIndex <= ReleaseNodeIndex && InNodeIndex < Nodes.Count)
|
|
{
|
|
var releaseNodes = Nodes.GetRange(InNodeIndex + 1, ReleaseNodeIndex - InNodeIndex);
|
|
if (releaseNodes.Where(n => n.IsAvoidableNode).Count() < trafficAvoidableNodeMax) return true;
|
|
var distance = GetDistance([.. Nodes.GetRange(InNodeIndex, ReleaseNodeIndex - InNodeIndex + 1)]);
|
|
distance -= Math.Sqrt(Math.Pow(Robot.VisualizationMsg.AgvPosition.X - Nodes[InNodeIndex].X, 2) + Math.Pow(Robot.VisualizationMsg.AgvPosition.Y - Nodes[InNodeIndex].Y, 2));
|
|
if (distance < trafficDistanceMax) return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
public TrafficNodeDto[] GetChekingNodes(double trafficAvoidableNodeMax, double trafficDistanceMin)
|
|
{
|
|
List<TrafficNodeDto> releaseNodes = [];
|
|
double distance = 0;
|
|
distance -= Math.Sqrt(Math.Pow(Nodes[InNodeIndex].X - Robot.VisualizationMsg.AgvPosition.X, 2) + Math.Pow(Nodes[InNodeIndex].Y - Robot.VisualizationMsg.AgvPosition.Y, 2));
|
|
int index = InNodeIndex < ReleaseNodeIndex ? InNodeIndex + 1 : InNodeIndex;
|
|
for (; index < Nodes.Count; index++)
|
|
{
|
|
releaseNodes.Add(Nodes[index]);
|
|
if (index < Nodes.Count - 1)
|
|
{
|
|
var remainingNodes = Nodes.GetRange(index + 1, Nodes.Count - (index + 1));
|
|
if (!remainingNodes.Any(n => n.IsAvoidableNode))
|
|
{
|
|
index = Nodes.Count;
|
|
break;
|
|
}
|
|
}
|
|
if (index > 0) distance += Math.Sqrt(Math.Pow(Nodes[index].X - Nodes[index - 1].X, 2) + Math.Pow(Nodes[index].Y - Nodes[index - 1].Y, 2));
|
|
if (distance < trafficDistanceMin || !Nodes[index].IsAvoidableNode) continue;
|
|
if (releaseNodes.Where(n => n.IsAvoidableNode).Count() >= trafficAvoidableNodeMax) break;
|
|
}
|
|
return index > ReleaseNodeIndex ? [.. Nodes.GetRange(ReleaseNodeIndex, index - ReleaseNodeIndex + (index < Nodes.Count ? 1 : 0))] : [];
|
|
}
|
|
|
|
public (TrafficSolutionState state, string message) UpdateGiveWay(TrafficNodeDto[] giveWayNodes, PathPlanner pathPlanner, MapManager map)
|
|
{
|
|
try
|
|
{
|
|
if (giveWayNodes.Length < 2) return (TrafficSolutionState.UnableResolve, "Lộ trình tránh đường không hợp lệ");
|
|
if (!giveWayNodes.Any(n => !Nodes.Contains(n)))
|
|
{
|
|
var giveNodeIndex = Nodes.IndexOf(giveWayNodes[^1]);
|
|
if (giveNodeIndex >= ReleaseNodeIndex)
|
|
{
|
|
ReleaseNode = giveWayNodes[^1];
|
|
State = TrafficSolutionState.Complete;
|
|
return (TrafficSolutionState.Complete, "");
|
|
}
|
|
}
|
|
var giveWayIndex = Nodes.IndexOf(giveWayNodes[0]);
|
|
var releaseNodeInGivewaysIndex = Array.FindIndex(giveWayNodes, n=> n.Id == ReleaseNode.Id);
|
|
if (giveWayIndex != -1)
|
|
{
|
|
List<TrafficNodeDto> subGiveWayNodes = [];
|
|
List<EdgeDto> subGiveWayEdges = [];
|
|
if (releaseNodeInGivewaysIndex != -1) subGiveWayNodes = giveWayNodes.ToList().GetRange(releaseNodeInGivewaysIndex, giveWayNodes.Length - releaseNodeInGivewaysIndex);
|
|
else if (giveWayIndex == ReleaseNodeIndex) subGiveWayNodes = [.. giveWayNodes];
|
|
else if (giveWayIndex < ReleaseNodeIndex)
|
|
{
|
|
subGiveWayNodes = Nodes.GetRange(giveWayIndex + 1, ReleaseNodeIndex - giveWayIndex);
|
|
subGiveWayNodes.Reverse();
|
|
subGiveWayNodes.AddRange(giveWayNodes);
|
|
}
|
|
else
|
|
{
|
|
subGiveWayNodes = Nodes.GetRange(ReleaseNodeIndex, giveWayIndex - ReleaseNodeIndex);
|
|
subGiveWayNodes.AddRange(giveWayNodes);
|
|
}
|
|
List<EdgeDto> edges = [..map.GetEdges(MapId, [..subGiveWayNodes.Select(n => new NodeDto
|
|
{
|
|
Id = n.Id,
|
|
Name = n.Name,
|
|
X = n.X,
|
|
Y = n.Y,
|
|
MapId = MapId,
|
|
})])];
|
|
if (edges.Count > 0)
|
|
{
|
|
double releaseTheta;
|
|
if (ReleaseNodeIndex > 0)
|
|
{
|
|
var (nearNodeX, nearNodeY) = MapEditorHelper.Curve(0.9, new EdgeCaculatorModel
|
|
{
|
|
TrajectoryDegree = Edges[ReleaseNodeIndex - 1].TrajectoryDegree,
|
|
ControlPoint1X = Edges[ReleaseNodeIndex - 1].ControlPoint1X,
|
|
ControlPoint1Y = Edges[ReleaseNodeIndex - 1].ControlPoint1Y,
|
|
ControlPoint2X = Edges[ReleaseNodeIndex - 1].ControlPoint2X,
|
|
ControlPoint2Y = Edges[ReleaseNodeIndex - 1].ControlPoint2Y,
|
|
X1 = Nodes[ReleaseNodeIndex - 1].X,
|
|
Y1 = Nodes[ReleaseNodeIndex - 1].Y,
|
|
X2 = Nodes[ReleaseNodeIndex].X,
|
|
Y2 = Nodes[ReleaseNodeIndex].Y,
|
|
});
|
|
var relaseForward = Math.Atan2(Nodes[ReleaseNodeIndex].Y - nearNodeY, Nodes[ReleaseNodeIndex].X - nearNodeX) * 180 / Math.PI;
|
|
var releaseBackward = Math.Atan2(nearNodeY - Nodes[ReleaseNodeIndex].Y, nearNodeX - Nodes[ReleaseNodeIndex].X) * 180 / Math.PI;
|
|
releaseTheta = Nodes[ReleaseNodeIndex - 1].Direction == RobotDirection.FORWARD ? relaseForward : releaseBackward;
|
|
}
|
|
else
|
|
{
|
|
var (nearNodeX, nearNodeY) = MapEditorHelper.Curve(0.1, new EdgeCaculatorModel
|
|
{
|
|
TrajectoryDegree = Edges[ReleaseNodeIndex].TrajectoryDegree,
|
|
ControlPoint1X = Edges[ReleaseNodeIndex].ControlPoint1X,
|
|
ControlPoint1Y = Edges[ReleaseNodeIndex].ControlPoint1Y,
|
|
ControlPoint2X = Edges[ReleaseNodeIndex].ControlPoint2X,
|
|
ControlPoint2Y = Edges[ReleaseNodeIndex].ControlPoint2Y,
|
|
X1 = Nodes[ReleaseNodeIndex].X,
|
|
Y1 = Nodes[ReleaseNodeIndex].Y,
|
|
X2 = Nodes[ReleaseNodeIndex + 1].X,
|
|
Y2 = Nodes[ReleaseNodeIndex + 1].Y,
|
|
});
|
|
var releaseBackward = Math.Atan2(Nodes[ReleaseNodeIndex].Y - nearNodeY, Nodes[ReleaseNodeIndex].X - nearNodeX) * 180 / Math.PI;
|
|
var relaseForward = Math.Atan2(nearNodeY - Nodes[ReleaseNodeIndex].Y, nearNodeX - Nodes[ReleaseNodeIndex].X) * 180 / Math.PI;
|
|
releaseTheta = Nodes[0].Direction == RobotDirection.FORWARD ? relaseForward : releaseBackward;
|
|
}
|
|
List<NodeDto> nodes = [..PathPlanner.CalculatorDirection(releaseTheta, [..subGiveWayNodes.Select(n => new NodeDto
|
|
{
|
|
Id = n.Id,
|
|
X = n.X,
|
|
Y = n.Y,
|
|
Name = n.Name,
|
|
MapId = MapId,
|
|
})], [..edges])];
|
|
GivewayNodes = [.. nodes.Select(n => new TrafficNodeDto
|
|
{
|
|
Id = n.Id,
|
|
X = n.X,
|
|
Y = n.Y,
|
|
Name = n.Name,
|
|
Direction = MapCompute.GetRobotDirection(n.Direction),
|
|
})];
|
|
//Console.WriteLine($"{Robot.SerialNumber} giveway: [{string.Join(",", GivewayNodes.Select(n => $"({n.Name} - {n.Direction})"))}]");
|
|
foreach (var node in GivewayNodes)
|
|
{
|
|
node.IsAvoidableNode = map.GetNegativeNodes(MapId, node.Id).Length > 2;
|
|
if (node.IsAvoidableNode)
|
|
{
|
|
node.AvoidablePaths = [.. map.GetNegativePaths(MapId, new() { Id = node.Id, X = node.X, Y = node.Y, Name = node.Name }, 2)
|
|
.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())];
|
|
}
|
|
}
|
|
GivewayEdges = [.. edges.Select(n => new TrafficEdgeDto
|
|
{
|
|
Id = n.Id,
|
|
ControlPoint1X = n.ControlPoint1X,
|
|
ControlPoint1Y = n.ControlPoint1Y,
|
|
ControlPoint2X = n.ControlPoint2X,
|
|
ControlPoint2Y = n.ControlPoint2Y,
|
|
StartNodeId = n.StartNodeId,
|
|
EndNodeId = n.EndNodeId,
|
|
TrajectoryDegree = n.TrajectoryDegree,
|
|
})];
|
|
State = TrafficSolutionState.GiveWay;
|
|
var angleForward = Math.Atan2(GivewayNodes[^1].Y - GivewayNodes[^2].Y, GivewayNodes[^1].X - GivewayNodes[^2].X) * 180 / Math.PI;
|
|
var angleBackward = Math.Atan2(GivewayNodes[^2].Y - GivewayNodes[^1].Y, GivewayNodes[^2].X - GivewayNodes[^1].X) * 180 / Math.PI;
|
|
RefreshPath(GivewayNodes[^1], GivewayNodes[^1].Direction == RobotDirection.FORWARD ? angleForward : angleBackward, pathPlanner, map);
|
|
return (TrafficSolutionState.GiveWay, "");
|
|
}
|
|
return (TrafficSolutionState.UnableResolve, $"Không tìm thấy edge yêu cầu phải tránh [{GivewayNodes[0].Name} - {GivewayNodes[1].Name}]");
|
|
}
|
|
return (TrafficSolutionState.UnableResolve, $"Không tìm thấy điểm xung đột {GivewayNodes[0].Name}");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
return (TrafficSolutionState.UnableResolve, $"Cập nhật lộ trình tránh đường xảy ra lỗi: {ex.Message}");
|
|
}
|
|
}
|
|
|
|
private void RefreshPath(TrafficNodeDto currentNode, double theta, PathPlanner planner, MapManager map)
|
|
{
|
|
RefreshPathState = RefreshPathState.Created;
|
|
var plannerTask = Task.Run(async () =>
|
|
{
|
|
RefreshPathState = RefreshPathState.Refreshing;
|
|
var path = await planner.Planning(currentNode.Id, theta, AgentModel.NavigationType, MapId, GoalNode.Id);
|
|
if (!path.IsSuccess)
|
|
{
|
|
RefreshPathState = RefreshPathState.Error;
|
|
Message = path.Message;
|
|
return;
|
|
}
|
|
if (path.Data.Nodes is null || path.Data.Edges is null || path.Data.Edges.Length == 0 || path.Data.Nodes.Length < 2)
|
|
{
|
|
RefreshPathState = RefreshPathState.Error;
|
|
Message = $"Đường dẫn tới đích [{GoalNode.Name} - {GoalNode.Id}] từ [{currentNode.Name} - {currentNode.Id}] không tồn tại";
|
|
return;
|
|
}
|
|
SubEdges = [..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,
|
|
})];
|
|
SubNodes = [..path.Data.Nodes.Select(n => new TrafficNodeDto()
|
|
{
|
|
Id = n.Id,
|
|
Name = n.Name,
|
|
X = n.X,
|
|
Y = n.Y,
|
|
Direction = MapCompute.GetRobotDirection(n.Direction)
|
|
})];
|
|
foreach (var node in SubNodes)
|
|
{
|
|
node.IsAvoidableNode = map.GetNegativeNodes(MapId, node.Id).Length > 2;
|
|
if (node.IsAvoidableNode)
|
|
{
|
|
node.AvoidablePaths = [.. map.GetNegativePaths(MapId, new() { Id = node.Id, X = node.X, Y = node.Y, Name = node.Name }, 2)
|
|
.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())];
|
|
}
|
|
}
|
|
//Console.WriteLine($"{Robot.SerialNumber} refresh path: [{string.Join(",", SubNodes.Select(n => $"({n.Name} - {n.Direction})"))}]");
|
|
RefreshPathState = RefreshPathState.Compeleted;
|
|
});
|
|
}
|
|
}
|