first commit -push
This commit is contained in:
dungtt
28
RobotNet.RobotManager/Services/Planner/AStar/AStarNode.cs
Normal file
28
RobotNet.RobotManager/Services/Planner/AStar/AStarNode.cs
Normal file
@@ -0,0 +1,28 @@
|
||||
namespace RobotNet.RobotManager.Services.Planner.AStar;
|
||||
|
||||
#nullable disable
|
||||
|
||||
public class AStarNode
|
||||
{
|
||||
public Guid Id { get; set; }
|
||||
public double X { get; set; }
|
||||
public double Y { get; set; }
|
||||
public double Cost { get; set; }
|
||||
public double Heuristic { get; set; }
|
||||
public double TotalCost => Cost + Heuristic;
|
||||
public string Name { get; set; }
|
||||
public AStarNode Parent { get; set; }
|
||||
public List<AStarNode> NegativeNodes { get; set; } = [];
|
||||
|
||||
public override bool Equals(object obj)
|
||||
{
|
||||
if (obj is AStarNode other)
|
||||
return Id == other.Id;
|
||||
return false;
|
||||
}
|
||||
|
||||
public override int GetHashCode()
|
||||
{
|
||||
return HashCode.Combine(Id);
|
||||
}
|
||||
}
|
||||
393
RobotNet.RobotManager/Services/Planner/AStar/AStarPathPlanner.cs
Normal file
393
RobotNet.RobotManager/Services/Planner/AStar/AStarPathPlanner.cs
Normal file
@@ -0,0 +1,393 @@
|
||||
using RobotNet.MapShares;
|
||||
using RobotNet.MapShares.Dtos;
|
||||
using RobotNet.MapShares.Enums;
|
||||
using RobotNet.RobotManager.Services.Planner.Space;
|
||||
using RobotNet.Shares;
|
||||
|
||||
namespace RobotNet.RobotManager.Services.Planner.AStar;
|
||||
|
||||
public class AStarPathPlanner(List<NodeDto> Nodes, List<EdgeDto> Edges)
|
||||
{
|
||||
public static NodeDto? GetCurveNode(double t, EdgeDto edge, NodeDto startNode, NodeDto endNode)
|
||||
{
|
||||
if (edge.TrajectoryDegree == TrajectoryDegree.Two)
|
||||
{
|
||||
return new()
|
||||
{
|
||||
X = (1 - t) * (1 - t) * startNode.X + 2 * t * (1 - t) * edge.ControlPoint1X + t * t * endNode.X,
|
||||
Y = (1 - t) * (1 - t) * startNode.Y + 2 * t * (1 - t) * edge.ControlPoint1Y + t * t * endNode.Y
|
||||
};
|
||||
}
|
||||
else if (edge.TrajectoryDegree == TrajectoryDegree.Three)
|
||||
{
|
||||
return new()
|
||||
{
|
||||
X = Math.Pow(1 - t, 3) * startNode.X + 3 * Math.Pow(1 - t, 2) * t * edge.ControlPoint1X + 3 * Math.Pow(t, 2) * (1 - t) * edge.ControlPoint2X + Math.Pow(t, 3) * endNode.X,
|
||||
Y = Math.Pow(1 - t, 3) * startNode.Y + 3 * Math.Pow(1 - t, 2) * t * edge.ControlPoint1Y + 3 * Math.Pow(t, 2) * (1 - t) * edge.ControlPoint2Y + Math.Pow(t, 3) * endNode.Y,
|
||||
};
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
public static double DistanceToCurveEdge(NodeDto nodeRef, EdgeDto edge, NodeDto startNode, NodeDto endNode)
|
||||
{
|
||||
double dMin = Math.Sqrt(Math.Pow(nodeRef.X - startNode.X, 2) + Math.Pow(nodeRef.Y - startNode.Y, 2));
|
||||
var length = MapEditorHelper.GetEdgeLength(new()
|
||||
{
|
||||
X1 = startNode.X,
|
||||
Y1 = startNode.Y,
|
||||
X2 = endNode.X,
|
||||
Y2 = endNode.Y,
|
||||
ControlPoint1X = edge.ControlPoint1X,
|
||||
ControlPoint1Y = edge.ControlPoint1Y,
|
||||
ControlPoint2X = edge.ControlPoint2X,
|
||||
ControlPoint2Y = edge.ControlPoint2Y,
|
||||
TrajectoryDegree = edge.TrajectoryDegree,
|
||||
});
|
||||
double step = 0.1 / (length == 0 ? 0.1 : length);
|
||||
|
||||
for (double t = 0; t <= 1; t += step)
|
||||
{
|
||||
var nodeCurve = GetCurveNode(t, edge, startNode, endNode);
|
||||
if (nodeCurve is null) continue;
|
||||
double d = Math.Sqrt(Math.Pow(nodeRef.X - nodeCurve.X, 2) + Math.Pow(nodeRef.Y - nodeCurve.Y, 2));
|
||||
if (d < dMin) dMin = d;
|
||||
}
|
||||
|
||||
return dMin;
|
||||
}
|
||||
|
||||
public static MessageResult<double> DistanceToEdge(NodeDto nodeRef, EdgeDto edge, NodeDto startNode, NodeDto endNode)
|
||||
{
|
||||
|
||||
if (edge.TrajectoryDegree == TrajectoryDegree.One)
|
||||
{
|
||||
double time = 0;
|
||||
var edgeLengthSquared = Math.Pow(startNode.X - endNode.X, 2) + Math.Pow(startNode.Y - endNode.Y, 2);
|
||||
if (edgeLengthSquared > 0)
|
||||
{
|
||||
time = Math.Max(0, Math.Min(1, ((nodeRef.X - startNode.X) * (endNode.X - startNode.X) + (nodeRef.Y - startNode.Y) * (endNode.Y - startNode.Y)) / edgeLengthSquared));
|
||||
}
|
||||
|
||||
double nearestX = startNode.X + time * (endNode.X - startNode.X);
|
||||
double nearestY = startNode.Y + time * (endNode.Y - startNode.Y);
|
||||
|
||||
return new(true) { Data = Math.Sqrt(Math.Pow(nodeRef.X - nearestX, 2) + Math.Pow(nodeRef.Y - nearestY, 2)) };
|
||||
}
|
||||
else
|
||||
{
|
||||
return new(true) { Data = DistanceToCurveEdge(nodeRef, edge, startNode, endNode) };
|
||||
}
|
||||
}
|
||||
|
||||
public EdgeDto? GetClosesEdge(NodeDto nodeRef, double limitDistance, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
double minDistance = double.MaxValue;
|
||||
EdgeDto? edgeResult = null;
|
||||
foreach (var edge in Edges)
|
||||
{
|
||||
if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) return null;
|
||||
var startNode = Nodes.FirstOrDefault(node => node.Id == edge.StartNodeId);
|
||||
var endNode = Nodes.FirstOrDefault(node => node.Id == edge.EndNodeId);
|
||||
if (startNode is null || endNode is null) continue;
|
||||
|
||||
var getDistance = DistanceToEdge(nodeRef, edge, startNode, endNode);
|
||||
if (getDistance.IsSuccess)
|
||||
{
|
||||
if (getDistance.Data < minDistance)
|
||||
{
|
||||
minDistance = getDistance.Data;
|
||||
edgeResult = edge;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (minDistance <= limitDistance) return edgeResult;
|
||||
else return null;
|
||||
}
|
||||
|
||||
private NodeDto? GetOnNode(double x, double y, double limitDistance, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
if (cancellationToken?.IsCancellationRequested == true) return null;
|
||||
KDTree KDTree = new(Nodes);
|
||||
return KDTree.FindNearest(x, y, limitDistance);
|
||||
}
|
||||
|
||||
private List<NodeDto> GetNegativeNode(Guid nodeId, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var node = Nodes.FirstOrDefault(p => p.Id == nodeId);
|
||||
if (node is null) return [];
|
||||
|
||||
var ListNodesNegative = new List<NodeDto>();
|
||||
var ListPaths = Edges.Where(p => p.EndNodeId == nodeId || p.StartNodeId == nodeId);
|
||||
foreach (var path in ListPaths)
|
||||
{
|
||||
if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) return [];
|
||||
if (path.StartNodeId == node.Id && (path.DirectionAllowed == DirectionAllowed.Both || path.DirectionAllowed == DirectionAllowed.Forward))
|
||||
{
|
||||
var nodeAdd = Nodes.FirstOrDefault(p => p.Id == path.EndNodeId);
|
||||
if (nodeAdd is not null) ListNodesNegative.Add(nodeAdd);
|
||||
continue;
|
||||
}
|
||||
if (path.EndNodeId == node.Id && (path.DirectionAllowed == DirectionAllowed.Both || path.DirectionAllowed == DirectionAllowed.Backward))
|
||||
{
|
||||
var nodeAdd = Nodes.FirstOrDefault(p => p.Id == path.StartNodeId);
|
||||
if (nodeAdd is not null) ListNodesNegative.Add(nodeAdd);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
return ListNodesNegative;
|
||||
}
|
||||
|
||||
private double GetNegativeCost(AStarNode currenNode, AStarNode negativeNode)
|
||||
{
|
||||
var negativeEdges = Edges.Where(e => e.StartNodeId == currenNode.Id && e.EndNodeId == negativeNode.Id || e.StartNodeId == negativeNode.Id && e.EndNodeId == currenNode.Id).ToList();
|
||||
double minDistance = double.MaxValue;
|
||||
foreach (var edge in negativeEdges)
|
||||
{
|
||||
var startNode = Nodes.FirstOrDefault(n => n.Id == edge.StartNodeId);
|
||||
var endNode = Nodes.FirstOrDefault(n => n.Id == edge.EndNodeId);
|
||||
if (startNode is null || endNode is null) return 0;
|
||||
var distance = MapEditorHelper.GetEdgeLength(new()
|
||||
{
|
||||
X1 = startNode.X,
|
||||
Y1 = startNode.Y,
|
||||
X2 = endNode.X,
|
||||
Y2 = endNode.Y,
|
||||
ControlPoint1X = edge.ControlPoint1X,
|
||||
ControlPoint1Y = edge.ControlPoint1Y,
|
||||
ControlPoint2X = edge.ControlPoint2X,
|
||||
ControlPoint2Y = edge.ControlPoint2Y,
|
||||
TrajectoryDegree = edge.TrajectoryDegree,
|
||||
});
|
||||
if (distance < minDistance) minDistance = distance;
|
||||
}
|
||||
return minDistance != double.MaxValue ? minDistance : 0;
|
||||
}
|
||||
|
||||
private List<AStarNode> GetNegativeAStarNode(AStarNode nodeCurrent, NodeDto endNode, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var possiblePointNegative = new List<AStarNode>();
|
||||
foreach (var nodeNegative in nodeCurrent.NegativeNodes)
|
||||
{
|
||||
nodeNegative.Parent = nodeCurrent;
|
||||
var ListNodesNegative = GetNegativeNode(nodeNegative.Id, cancellationToken);
|
||||
foreach (var item in ListNodesNegative)
|
||||
{
|
||||
if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) return [];
|
||||
nodeNegative.NegativeNodes.Add(new AStarNode()
|
||||
{
|
||||
Id = item.Id,
|
||||
X = item.X,
|
||||
Y = item.Y,
|
||||
Name = item.Name,
|
||||
});
|
||||
}
|
||||
var cost = GetNegativeCost(nodeCurrent, nodeNegative);
|
||||
nodeNegative.Cost = (cost > 0 ? cost : Math.Sqrt(Math.Pow(nodeCurrent.X - nodeNegative.X, 2) + Math.Pow(nodeCurrent.Y - nodeNegative.Y, 2))) + nodeCurrent.Cost;
|
||||
nodeNegative.Heuristic = Math.Abs(endNode.X - nodeNegative.X) + Math.Abs(endNode.Y - nodeNegative.Y);
|
||||
possiblePointNegative.Add(nodeNegative);
|
||||
}
|
||||
return possiblePointNegative;
|
||||
}
|
||||
|
||||
private List<AStarNode> Find(AStarNode startNode, NodeDto endNode, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
try
|
||||
{
|
||||
var activeNodes = new PriorityQueue<AStarNode>((a, b) => a.TotalCost.CompareTo(b.TotalCost));
|
||||
var visitedNodes = new HashSet<AStarNode>();
|
||||
List<AStarNode> Path = [];
|
||||
activeNodes.Enqueue(startNode);
|
||||
|
||||
while (activeNodes.Count != 0 && (!cancellationToken.HasValue || !cancellationToken.Value.IsCancellationRequested))
|
||||
{
|
||||
var checkNode = activeNodes.Dequeue();
|
||||
if (checkNode.Id == endNode.Id)
|
||||
{
|
||||
var node = checkNode;
|
||||
while (node != null)
|
||||
{
|
||||
if (cancellationToken.HasValue && cancellationToken.Value.IsCancellationRequested) return [];
|
||||
Path.Add(node);
|
||||
node = node.Parent;
|
||||
}
|
||||
return Path;
|
||||
}
|
||||
|
||||
visitedNodes.Add(checkNode);
|
||||
|
||||
var ListNodeNegative = GetNegativeAStarNode(checkNode, endNode, cancellationToken);
|
||||
foreach (var node in ListNodeNegative)
|
||||
{
|
||||
if (visitedNodes.TryGetValue(node, out AStarNode? value) && value is not null)
|
||||
{
|
||||
if (value.TotalCost > node.TotalCost)
|
||||
{
|
||||
visitedNodes.Remove(value);
|
||||
activeNodes.Enqueue(node);
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
var activeNode = activeNodes.Items.FirstOrDefault(n => n.Id == node.Id);
|
||||
if (activeNode is not null && activeNode.TotalCost > node.TotalCost)
|
||||
{
|
||||
activeNodes.Items.Remove(activeNode);
|
||||
activeNodes.Enqueue(node);
|
||||
}
|
||||
else
|
||||
{
|
||||
activeNodes.Enqueue(node);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return [];
|
||||
}
|
||||
catch
|
||||
{
|
||||
return [];
|
||||
}
|
||||
}
|
||||
|
||||
public MessageResult<List<NodeDto>> Planning(double x, double y, NodeDto goal, double maxDistanceToEdge, double maxDistanceToNode, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var Path = new List<NodeDto>();
|
||||
|
||||
AStarNode RobotNode = new()
|
||||
{
|
||||
Id = Guid.NewGuid(),
|
||||
X = x,
|
||||
Y = y,
|
||||
Name = "RobotCurrentNode",
|
||||
};
|
||||
var closesNode = GetOnNode(x, y, maxDistanceToNode);
|
||||
if (closesNode is not null)
|
||||
{
|
||||
if (closesNode.Id == goal.Id) return new(true, "Robot đang đứng tại điểm đích") { Data = [goal] };
|
||||
RobotNode.Name = closesNode.Name;
|
||||
RobotNode.Id = closesNode.Id;
|
||||
RobotNode.X = closesNode.X;
|
||||
RobotNode.Y = closesNode.Y;
|
||||
|
||||
foreach (var negativeNode in GetNegativeNode(RobotNode.Id, cancellationToken))
|
||||
{
|
||||
var cost = GetNegativeCost(RobotNode, new() { Id = negativeNode.Id, X = negativeNode.X, Y = negativeNode.Y });
|
||||
RobotNode.NegativeNodes.Add(new()
|
||||
{
|
||||
Id = negativeNode.Id,
|
||||
X = negativeNode.X,
|
||||
Y = negativeNode.Y,
|
||||
Name = negativeNode.Name,
|
||||
Cost = cost > 0 ? cost : Math.Sqrt(Math.Pow(RobotNode.X - negativeNode.X, 2) + Math.Pow(RobotNode.Y - negativeNode.Y, 2)),
|
||||
Heuristic = Math.Abs(goal.X - negativeNode.X) + Math.Abs(goal.Y - negativeNode.Y),
|
||||
});
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
var closesEdge = GetClosesEdge(new() { X = x, Y = y }, maxDistanceToEdge, cancellationToken);
|
||||
if (closesEdge is null)
|
||||
{
|
||||
return new(false, "Robot đang quá xa tuyến đường");
|
||||
}
|
||||
|
||||
var startNodeForward = Nodes.FirstOrDefault(p => p.Id == closesEdge.StartNodeId);
|
||||
var startNodeBackward = Nodes.FirstOrDefault(p => p.Id == closesEdge.EndNodeId);
|
||||
if (startNodeForward is null || startNodeBackward is null)
|
||||
{
|
||||
return new(false, "Dữ liệu lỗi: điểm chặn của edge gần nhất không tồn tại");
|
||||
}
|
||||
if (startNodeForward.Id == goal.Id && (closesEdge.DirectionAllowed == DirectionAllowed.Both || closesEdge.DirectionAllowed == DirectionAllowed.Backward))
|
||||
{
|
||||
Path.Add(startNodeBackward);
|
||||
Path.Add(startNodeForward);
|
||||
return new(true) { Data = Path };
|
||||
}
|
||||
if (startNodeBackward.Id == goal.Id && (closesEdge.DirectionAllowed == DirectionAllowed.Both || closesEdge.DirectionAllowed == DirectionAllowed.Forward))
|
||||
{
|
||||
Path.Add(startNodeForward);
|
||||
Path.Add(startNodeBackward);
|
||||
return new(true) { Data = Path };
|
||||
}
|
||||
if (closesEdge.DirectionAllowed == DirectionAllowed.Both || closesEdge.DirectionAllowed == DirectionAllowed.Backward)
|
||||
{
|
||||
RobotNode.NegativeNodes.Add(new()
|
||||
{
|
||||
Id = startNodeForward.Id,
|
||||
X = startNodeForward.X,
|
||||
Y = startNodeForward.Y,
|
||||
Name = startNodeForward.Name,
|
||||
Cost = Math.Sqrt(Math.Pow(RobotNode.X - startNodeForward.X, 2) + Math.Pow(RobotNode.Y - startNodeForward.Y, 2)),
|
||||
Heuristic = Math.Abs(goal.X - startNodeForward.X) + Math.Abs(goal.Y - startNodeForward.Y),
|
||||
});
|
||||
}
|
||||
if (closesEdge.DirectionAllowed == DirectionAllowed.Both || closesEdge.DirectionAllowed == DirectionAllowed.Forward)
|
||||
{
|
||||
RobotNode.NegativeNodes.Add(new()
|
||||
{
|
||||
Id = startNodeBackward.Id,
|
||||
X = startNodeBackward.X,
|
||||
Y = startNodeBackward.Y,
|
||||
Name = startNodeBackward.Name,
|
||||
Cost = Math.Sqrt(Math.Pow(RobotNode.X - startNodeBackward.X, 2) + Math.Pow(RobotNode.Y - startNodeBackward.Y, 2)),
|
||||
Heuristic = Math.Abs(goal.X - startNodeBackward.X) + Math.Abs(goal.Y - startNodeBackward.Y),
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
if (RobotNode.NegativeNodes.Count < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{x}, {y}]");
|
||||
|
||||
var path = Find(RobotNode, goal, cancellationToken);
|
||||
if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) return new(false, "Yêu cầu hủy bỏ");
|
||||
if (path is null || path.Count < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{x}, {y}]");
|
||||
path.Reverse();
|
||||
foreach (var node in path)
|
||||
{
|
||||
if (node.Name == "RobotCurrentNode")
|
||||
{
|
||||
Path.Add(new()
|
||||
{
|
||||
Id = RobotNode.Id,
|
||||
Name = RobotNode.Name,
|
||||
X = RobotNode.X,
|
||||
Y = RobotNode.Y,
|
||||
});
|
||||
continue;
|
||||
}
|
||||
var nodedb = Nodes.FirstOrDefault(p => p.Id == node.Id);
|
||||
if (nodedb is null) return new(false, "Dữ liệu bản đồ có lỗi");
|
||||
Path.Add(nodedb);
|
||||
}
|
||||
return new(true) { Data = Path };
|
||||
}
|
||||
|
||||
public MessageResult<NodeDto[]> Planning(NodeDto startNode, NodeDto goal, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var Path = new List<NodeDto>();
|
||||
var currentNode = new AStarNode
|
||||
{
|
||||
Id = startNode.Id,
|
||||
X = startNode.X,
|
||||
Y = startNode.Y,
|
||||
NegativeNodes = [..GetNegativeNode(startNode.Id).Select(n => new AStarNode
|
||||
{
|
||||
Id = n.Id,
|
||||
Name = n.Name,
|
||||
X = n.X,
|
||||
Y = n.Y,
|
||||
})],
|
||||
};
|
||||
var path = Find(currentNode, goal, cancellationToken);
|
||||
if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) return new(false, "Yêu cầu hủy bỏ");
|
||||
if (path is null || path.Count < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{startNode.Name} - {startNode.Id}]");
|
||||
path.Reverse();
|
||||
foreach (var node in path)
|
||||
{
|
||||
var nodedb = Nodes.FirstOrDefault(p => p.Id == node.Id);
|
||||
if (nodedb is null) return new(false, "Dữ liệu bản đồ có lỗi");
|
||||
Path.Add(nodedb);
|
||||
}
|
||||
return new(true) { Data = [.. Path] };
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user