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] };
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,266 @@
|
||||
using RobotNet.MapShares;
|
||||
using RobotNet.MapShares.Dtos;
|
||||
using RobotNet.MapShares.Enums;
|
||||
using RobotNet.RobotManager.Services.Planner.AStar;
|
||||
using RobotNet.RobotManager.Services.Planner.Space;
|
||||
using RobotNet.RobotShares.Enums;
|
||||
using RobotNet.Shares;
|
||||
|
||||
namespace RobotNet.RobotManager.Services.Planner.Differential;
|
||||
|
||||
public class DifferentialPathPlanner : IPathPlanner
|
||||
{
|
||||
private List<NodeDto> Nodes = [];
|
||||
private List<EdgeDto> Edges = [];
|
||||
private const double ReverseDirectionAngleDegree = 89;
|
||||
private const double Ratio = 0.1;
|
||||
|
||||
private readonly PathPlanningOptions Options = new()
|
||||
{
|
||||
LimitDistanceToEdge = 1,
|
||||
LimitDistanceToNode = 0.3,
|
||||
ResolutionSplit = 0.1,
|
||||
};
|
||||
|
||||
public void SetData(NodeDto[] nodes, EdgeDto[] edges)
|
||||
{
|
||||
Nodes = [.. nodes];
|
||||
Edges = [.. edges];
|
||||
}
|
||||
|
||||
public void SetOptions(PathPlanningOptions options)
|
||||
{
|
||||
Options.LimitDistanceToNode = options.LimitDistanceToNode;
|
||||
Options.LimitDistanceToEdge = options.LimitDistanceToEdge;
|
||||
Options.ResolutionSplit = options.ResolutionSplit;
|
||||
}
|
||||
|
||||
public static RobotDirection[] GetRobotDirectionInPath(RobotDirection currentDirection, List<NodeDto> nodeplanning, List<EdgeDto> edgePlanning)
|
||||
{
|
||||
RobotDirection[] RobotDirectionInNode = new RobotDirection[nodeplanning.Count];
|
||||
if (nodeplanning.Count > 0) RobotDirectionInNode[0] = currentDirection;
|
||||
if (nodeplanning.Count > 2)
|
||||
{
|
||||
for (int i = 1; i < nodeplanning.Count - 1; i++)
|
||||
{
|
||||
NodeDto startNode = MapEditorHelper.GetNearByNode(nodeplanning[i], nodeplanning[i - 1], edgePlanning[i - 1], Ratio);
|
||||
NodeDto endNode = MapEditorHelper.GetNearByNode(nodeplanning[i], nodeplanning[i + 1], edgePlanning[i], Ratio); ;
|
||||
var angle = MapEditorHelper.GetAngle(nodeplanning[i], startNode, endNode);
|
||||
if (angle < ReverseDirectionAngleDegree) RobotDirectionInNode[i] = RobotDirectionInNode[i - 1] == RobotDirection.FORWARD ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
|
||||
else RobotDirectionInNode[i] = RobotDirectionInNode[i - 1];
|
||||
}
|
||||
}
|
||||
if (nodeplanning.Count > 1) RobotDirectionInNode[^1] = RobotDirectionInNode[^2];
|
||||
return RobotDirectionInNode;
|
||||
}
|
||||
|
||||
public static RobotDirection GetRobotDirection(NodeDto currentNode, NodeDto nearNode, EdgeDto edge, double robotInNodeAngle, bool isReverse)
|
||||
{
|
||||
NodeDto NearNode = MapEditorHelper.GetNearByNode(currentNode, nearNode, edge, Ratio);
|
||||
|
||||
var RobotNearNode = new NodeDto()
|
||||
{
|
||||
X = currentNode.X + Math.Cos(robotInNodeAngle * Math.PI / 180),
|
||||
Y = currentNode.Y + Math.Sin(robotInNodeAngle * Math.PI / 180),
|
||||
};
|
||||
var angle = MapEditorHelper.GetAngle(currentNode, NearNode, RobotNearNode);
|
||||
if (angle > ReverseDirectionAngleDegree) return isReverse ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
|
||||
else return isReverse ? RobotDirection.FORWARD : RobotDirection.BACKWARD;
|
||||
}
|
||||
|
||||
private EdgeDto[] GetEdgesPlanning(NodeDto[] nodes)
|
||||
{
|
||||
var EdgesPlanning = new List<EdgeDto>();
|
||||
for (int i = 0; i < nodes.Length - 1; i++)
|
||||
{
|
||||
var edge = Edges.FirstOrDefault(e => e.StartNodeId == nodes[i].Id && e.EndNodeId == nodes[i + 1].Id ||
|
||||
e.EndNodeId == nodes[i].Id && e.StartNodeId == nodes[i + 1].Id);
|
||||
if (edge is null)
|
||||
{
|
||||
if (i != 0) return [];
|
||||
EdgesPlanning.Add(new EdgeDto()
|
||||
{
|
||||
Id = Guid.NewGuid(),
|
||||
StartNodeId = nodes[i].Id,
|
||||
EndNodeId = nodes[i + 1].Id,
|
||||
DirectionAllowed = DirectionAllowed.Both,
|
||||
TrajectoryDegree = TrajectoryDegree.One,
|
||||
});
|
||||
continue;
|
||||
}
|
||||
bool isReverse = nodes[i].Id != edge.StartNodeId && edge.TrajectoryDegree == TrajectoryDegree.Three;
|
||||
EdgesPlanning.Add(new()
|
||||
{
|
||||
Id = edge.Id,
|
||||
StartNodeId = nodes[i].Id,
|
||||
EndNodeId = nodes[i + 1].Id,
|
||||
DirectionAllowed = edge.DirectionAllowed,
|
||||
TrajectoryDegree = edge.TrajectoryDegree,
|
||||
ControlPoint1X = isReverse ? edge.ControlPoint2X : edge.ControlPoint1X,
|
||||
ControlPoint1Y = isReverse ? edge.ControlPoint2Y : edge.ControlPoint1Y,
|
||||
ControlPoint2X = isReverse ? edge.ControlPoint1X : edge.ControlPoint2X,
|
||||
ControlPoint2Y = isReverse ? edge.ControlPoint1Y : edge.ControlPoint2Y
|
||||
});
|
||||
}
|
||||
return [.. EdgesPlanning];
|
||||
}
|
||||
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanning(double x, double y, double theta, Guid goalId, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
|
||||
if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
|
||||
|
||||
var AStarPathPlanner = new AStarPathPlanner(Nodes, Edges);
|
||||
var Path = AStarPathPlanner.Planning(x,
|
||||
y,
|
||||
new NodeDto() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
|
||||
Options.LimitDistanceToEdge,
|
||||
Options.LimitDistanceToNode,
|
||||
cancellationToken);
|
||||
if (!Path.IsSuccess) return new(false, Path.Message);
|
||||
if (Path.Data is null || Path.Data.Count < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{x}, {y}, {theta}]");
|
||||
if (Path.Data.Count == 1) return new(true, "Robot đang đứng ở điểm đích") { Data = ([goal], []) };
|
||||
|
||||
var edgeplannings = GetEdgesPlanning([..Path.Data]);
|
||||
|
||||
RobotDirection CurrenDirection = GetRobotDirection(Path.Data[0], Path.Data[1], edgeplannings[0], theta, true);
|
||||
var FinalDirection = GetRobotDirectionInPath(CurrenDirection, Path.Data, [..edgeplannings]);
|
||||
foreach (var item in Path.Data)
|
||||
{
|
||||
item.Direction = MapCompute.GetNodeDirection(FinalDirection[Path.Data.IndexOf(item)]);
|
||||
}
|
||||
|
||||
return new(true)
|
||||
{
|
||||
Data = ([.. Path.Data], [.. edgeplannings])
|
||||
};
|
||||
}
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithAngle(double x, double y, double theta, Guid goalId, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
|
||||
if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
|
||||
|
||||
var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
|
||||
if (!basicPath.IsSuccess) return basicPath;
|
||||
if(basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([goal], []) };
|
||||
|
||||
RobotDirection goalDirection = GetRobotDirection(basicPath.Data.Nodes[^1], basicPath.Data.Nodes[^2], basicPath.Data.Edges[^1], goal.Theta, false);
|
||||
if (MapCompute.GetNodeDirection(goalDirection) == basicPath.Data.Nodes[^1].Direction) return basicPath;
|
||||
foreach (var node in basicPath.Data.Nodes)
|
||||
{
|
||||
node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
|
||||
}
|
||||
|
||||
return basicPath;
|
||||
}
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithStartDirection(double x, double y, double theta, Guid goalId, RobotDirection startDiretion = RobotDirection.NONE, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
|
||||
if (!basicPath.IsSuccess) return basicPath;
|
||||
if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
|
||||
|
||||
if (MapCompute.GetNodeDirection(startDiretion) == basicPath.Data.Nodes[0].Direction || startDiretion == RobotDirection.NONE) return basicPath;
|
||||
foreach (var node in basicPath.Data.Nodes)
|
||||
{
|
||||
node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
|
||||
}
|
||||
|
||||
return basicPath;
|
||||
}
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithFinalDirection(double x, double y, double theta, Guid goalId, RobotDirection goalDirection = RobotDirection.NONE, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
|
||||
if (!basicPath.IsSuccess) return basicPath;
|
||||
if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
|
||||
|
||||
if (MapCompute.GetNodeDirection(goalDirection) == basicPath.Data.Nodes[^1].Direction || goalDirection == RobotDirection.NONE) return basicPath;
|
||||
foreach (var node in basicPath.Data.Nodes)
|
||||
{
|
||||
node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
|
||||
}
|
||||
|
||||
return basicPath;
|
||||
}
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanning(Guid startNodeId, double theta, Guid goalId, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
|
||||
if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
|
||||
|
||||
var startNode = Nodes.FirstOrDefault(n => n.Id == startNodeId);
|
||||
if (startNode is null) return new(false, $"Điểm bắt đầu {startNodeId} không tồn tại trong map");
|
||||
|
||||
var AStarPathPlanner = new AStarPathPlanner(Nodes, Edges);
|
||||
var Path = AStarPathPlanner.Planning(startNode, goal, cancellationToken);
|
||||
if (!Path.IsSuccess) return new(false, Path.Message);
|
||||
if (Path.Data is null || Path.Data.Length < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{startNode.Name} - {startNode.Id}]");
|
||||
if (Path.Data.Length == 1) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
|
||||
|
||||
var edgeplannings = GetEdgesPlanning([.. Path.Data]);
|
||||
|
||||
RobotDirection CurrenDirection = GetRobotDirection(Path.Data[0], Path.Data[1], edgeplannings[0], theta, true);
|
||||
var FinalDirection = GetRobotDirectionInPath(CurrenDirection, [.. Path.Data], [.. edgeplannings]);
|
||||
for (int i = 0; i < Path.Data.Length; i++)
|
||||
{
|
||||
Path.Data[i].Direction = MapCompute.GetNodeDirection(FinalDirection[i]);
|
||||
}
|
||||
|
||||
return new(true)
|
||||
{
|
||||
Data = ([.. Path.Data], [.. edgeplannings])
|
||||
};
|
||||
}
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithAngle(Guid startNodeId, double theta, Guid goalId, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
|
||||
if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
|
||||
|
||||
var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
|
||||
if (!basicPath.IsSuccess) return basicPath;
|
||||
if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
|
||||
|
||||
RobotDirection goalDirection = GetRobotDirection(basicPath.Data.Nodes[^1], basicPath.Data.Nodes[^2], basicPath.Data.Edges[^1], goal.Theta, false);
|
||||
if (MapCompute.GetNodeDirection(goalDirection) == basicPath.Data.Nodes[^1].Direction) return basicPath;
|
||||
foreach (var node in basicPath.Data.Nodes)
|
||||
{
|
||||
node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
|
||||
}
|
||||
|
||||
return basicPath;
|
||||
}
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithStartDirection(Guid startNodeId, double theta, Guid goalId, RobotDirection startDiretion = RobotDirection.NONE, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
|
||||
if (!basicPath.IsSuccess) return basicPath;
|
||||
if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
|
||||
|
||||
if (MapCompute.GetNodeDirection(startDiretion) == basicPath.Data.Nodes[0].Direction || startDiretion == RobotDirection.NONE) return basicPath;
|
||||
foreach (var node in basicPath.Data.Nodes)
|
||||
{
|
||||
node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
|
||||
}
|
||||
|
||||
return basicPath;
|
||||
}
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithFinalDirection(Guid startNodeId, double theta, Guid goalId, RobotDirection goalDirection = RobotDirection.NONE, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
|
||||
if (!basicPath.IsSuccess) return basicPath;
|
||||
if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
|
||||
|
||||
if (MapCompute.GetNodeDirection(goalDirection) == basicPath.Data.Nodes[^1].Direction || goalDirection == RobotDirection.NONE) return basicPath;
|
||||
foreach (var node in basicPath.Data.Nodes)
|
||||
{
|
||||
node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
|
||||
}
|
||||
|
||||
return basicPath;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,596 @@
|
||||
using RobotNet.MapShares;
|
||||
using RobotNet.MapShares.Dtos;
|
||||
using RobotNet.MapShares.Enums;
|
||||
using RobotNet.RobotManager.Services.Planner.AStar;
|
||||
using RobotNet.RobotManager.Services.Planner.Space;
|
||||
using RobotNet.RobotShares.Enums;
|
||||
using RobotNet.Shares;
|
||||
|
||||
namespace RobotNet.RobotManager.Services.Planner.Fokrlift;
|
||||
|
||||
public class ForkliftPathPlanner : IPathPlanner
|
||||
{
|
||||
private List<NodeDto> Nodes = [];
|
||||
private List<EdgeDto> Edges = [];
|
||||
private const double ReverseDirectionAngleDegree = 89;
|
||||
private const double RobotDirectionWithAngle = 90;
|
||||
private const double Ratio = 0.1;
|
||||
|
||||
private readonly PathPlanningOptions Options = new()
|
||||
{
|
||||
LimitDistanceToEdge = 1,
|
||||
LimitDistanceToNode = 0.3,
|
||||
ResolutionSplit = 0.1,
|
||||
};
|
||||
|
||||
public void SetData(NodeDto[] nodes, EdgeDto[] edges)
|
||||
{
|
||||
Nodes = [.. nodes];
|
||||
Edges = [.. edges];
|
||||
}
|
||||
|
||||
public void SetOptions(PathPlanningOptions options)
|
||||
{
|
||||
Options.LimitDistanceToNode = options.LimitDistanceToNode;
|
||||
Options.LimitDistanceToEdge = options.LimitDistanceToEdge;
|
||||
Options.ResolutionSplit = options.ResolutionSplit;
|
||||
}
|
||||
|
||||
private static bool TStructureExisted(List<TStructure> TStructures, NodeDto node1, NodeDto node2, NodeDto node3)
|
||||
{
|
||||
var TStructureExistedStep1 = TStructures.Where(ts => ts.Node1 == node1 || ts.Node2 == node1 || ts.Node3 == node1).ToList();
|
||||
if (TStructureExistedStep1.Count != 0)
|
||||
{
|
||||
var TStructureExistedStep2 = TStructureExistedStep1.Where(ts => ts.Node1 == node2 || ts.Node2 == node2 || ts.Node3 == node2).ToList();
|
||||
if (TStructureExistedStep2.Count != 0)
|
||||
{
|
||||
var TStructureExistedStep3 = TStructureExistedStep2.Where(ts => ts.Node1 == node3 || ts.Node2 == node3 || ts.Node3 == node3).ToList();
|
||||
if (TStructureExistedStep3.Count != 0) return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
private List<TStructure> GetTStructure()
|
||||
{
|
||||
List<TStructure> TStructures = [];
|
||||
foreach (var node in Nodes)
|
||||
{
|
||||
var inEdges = Edges.Where(edge => edge.StartNodeId == node.Id || edge.EndNodeId == node.Id).ToList();
|
||||
if (inEdges.Count < 2) continue;
|
||||
List<NodeDto> inNodes = [];
|
||||
foreach (var edge in inEdges)
|
||||
{
|
||||
var endNode = Nodes.FirstOrDefault(n => n.Id == (node.Id == edge.EndNodeId ? edge.StartNodeId : edge.EndNodeId));
|
||||
if (endNode is null) continue;
|
||||
inNodes.Add(endNode);
|
||||
}
|
||||
for (int i = 0; i < inNodes.Count - 1; i++)
|
||||
{
|
||||
for (int j = i + 1; j < inNodes.Count; j++)
|
||||
{
|
||||
if (TStructureExisted(TStructures, node, inNodes[i], inNodes[j])) continue;
|
||||
var edgeT = Edges.FirstOrDefault(e => (e.StartNodeId == inNodes[i].Id && e.EndNodeId == inNodes[j].Id) ||
|
||||
(e.EndNodeId == inNodes[i].Id && e.StartNodeId == inNodes[j].Id));
|
||||
var edge1 = inEdges.FirstOrDefault(edge => edge.StartNodeId == inNodes[i].Id || edge.EndNodeId == inNodes[i].Id);
|
||||
var edge2 = inEdges.FirstOrDefault(edge => edge.StartNodeId == inNodes[j].Id || edge.EndNodeId == inNodes[j].Id);
|
||||
if (edgeT is null || edge1 is null || edge2 is null) continue;
|
||||
if (edgeT.TrajectoryDegree == TrajectoryDegree.One &&
|
||||
edge1.TrajectoryDegree == TrajectoryDegree.One &&
|
||||
edge2.TrajectoryDegree == TrajectoryDegree.One) continue;
|
||||
|
||||
TStructures.Add(new()
|
||||
{
|
||||
Node1 = node,
|
||||
Node2 = inNodes[i],
|
||||
Node3 = inNodes[j],
|
||||
Edge12 = edge1,
|
||||
Edge13 = edge2,
|
||||
Edge23 = edgeT,
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
return TStructures;
|
||||
}
|
||||
|
||||
public static RobotDirection[] GetRobotDirectionInPath(RobotDirection currentDirection, NodeDto[] nodeplanning, EdgeDto[] edgePlanning, double reverseDirectionAngle)
|
||||
{
|
||||
RobotDirection[] RobotDirectionInNode = new RobotDirection[nodeplanning.Length];
|
||||
if (nodeplanning.Length > 0) RobotDirectionInNode[0] = currentDirection;
|
||||
if (nodeplanning.Length > 2)
|
||||
{
|
||||
for (int i = 1; i < nodeplanning.Length - 1; i++)
|
||||
{
|
||||
NodeDto startNode = MapEditorHelper.GetNearByNode(nodeplanning[i], nodeplanning[i - 1], edgePlanning[i - 1], Ratio);
|
||||
NodeDto endNode = MapEditorHelper.GetNearByNode(nodeplanning[i], nodeplanning[i + 1], edgePlanning[i], Ratio); ;
|
||||
var angle = MapEditorHelper.GetAngle(nodeplanning[i], startNode, endNode);
|
||||
if (angle < reverseDirectionAngle) RobotDirectionInNode[i] = RobotDirectionInNode[i - 1] == RobotDirection.FORWARD ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
|
||||
else RobotDirectionInNode[i] = RobotDirectionInNode[i - 1];
|
||||
}
|
||||
}
|
||||
if (nodeplanning.Length > 1) RobotDirectionInNode[^1] = RobotDirectionInNode[^2];
|
||||
return RobotDirectionInNode;
|
||||
}
|
||||
|
||||
public static RobotDirection GetRobotDirection(NodeDto currentNode, NodeDto futureNode, EdgeDto edge, double robotTheta, double reverseDirectionAngle, bool inGoal)
|
||||
{
|
||||
NodeDto NearNode = MapEditorHelper.GetNearByNode(currentNode, futureNode, edge, Ratio);
|
||||
|
||||
var RobotDirectionNearNode = new NodeDto()
|
||||
{
|
||||
X = currentNode.X + Math.Cos(robotTheta * Math.PI / 180),
|
||||
Y = currentNode.Y + Math.Sin(robotTheta * Math.PI / 180),
|
||||
};
|
||||
var angle = MapEditorHelper.GetAngle(currentNode, NearNode, RobotDirectionNearNode);
|
||||
if (angle > reverseDirectionAngle) return inGoal ? RobotDirection.FORWARD : RobotDirection.BACKWARD;
|
||||
else return inGoal ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
|
||||
}
|
||||
|
||||
private static double GetNodeAngle(NodeDto node, NodeDto lastNode, EdgeDto edge)
|
||||
{
|
||||
NodeDto NearNode = MapEditorHelper.GetNearByNode(node, lastNode, edge, Ratio);
|
||||
return Math.Atan2(node.Y - NearNode.Y, node.X - NearNode.X) * 180 / Math.PI;
|
||||
}
|
||||
|
||||
private static (bool IsSuccess, NodeDto? intraNode, TStructure? tstructure) IsReverse(NodeDto currentNode, NodeDto olderNode, NodeDto? futureNode, EdgeDto olderedge, EdgeDto? futureedge, double startAngle, List<TStructure> tstructures)
|
||||
{
|
||||
var tstructures1 = tstructures.Where(t => t.Node1.Id == currentNode.Id || t.Node2.Id == currentNode.Id || t.Node3.Id == currentNode.Id).ToList();
|
||||
if (tstructures1 is null || tstructures1.Count < 1) return (false, null, null);
|
||||
var tstructures2 = tstructures1.Where(t => t.Node1.Id == olderNode.Id || t.Node2.Id == olderNode.Id || t.Node3.Id == olderNode.Id).ToList();
|
||||
if (tstructures2 is null || tstructures2.Count < 1) return (false, null, null);
|
||||
foreach (var ts in tstructures2)
|
||||
{
|
||||
var midleReverse = ts.IsDriectionReverse(currentNode, olderNode);
|
||||
var intraNode = ts.GetIntraNode(currentNode, olderNode);
|
||||
if (intraNode is null) continue;
|
||||
|
||||
if (!ts.IsAccessDirection(olderNode, intraNode) || !ts.IsAccessDirection(intraNode, currentNode)) continue;
|
||||
|
||||
var CurrentDirection = GetRobotDirection(olderNode, currentNode, olderedge, startAngle, ReverseDirectionAngleDegree, false);
|
||||
var intraEdge = ts.GetEdge(olderNode, intraNode);
|
||||
if (intraEdge is null) continue;
|
||||
var ReverseDirection = GetRobotDirection(olderNode, intraNode, intraEdge, startAngle, ReverseDirectionAngleDegree, false);
|
||||
bool firstReverse = ReverseDirection != CurrentDirection;
|
||||
|
||||
bool endReverse = false;
|
||||
if (futureNode is not null && futureedge is not null)
|
||||
{
|
||||
startAngle = GetNodeAngle(currentNode, olderNode, olderedge);
|
||||
CurrentDirection = GetRobotDirection(currentNode, futureNode, futureedge, startAngle, ReverseDirectionAngleDegree, true);
|
||||
intraEdge = ts.GetEdge(currentNode, intraNode);
|
||||
if (intraEdge is null) continue;
|
||||
startAngle = GetNodeAngle(currentNode, intraNode, intraEdge);
|
||||
ReverseDirection = GetRobotDirection(currentNode, futureNode, futureedge, startAngle, ReverseDirectionAngleDegree, true);
|
||||
endReverse = ReverseDirection != CurrentDirection;
|
||||
}
|
||||
|
||||
if (!midleReverse)
|
||||
{
|
||||
if ((!firstReverse && !endReverse) || (firstReverse && endReverse)) continue;
|
||||
}
|
||||
else
|
||||
{
|
||||
if ((firstReverse && !endReverse) || (!firstReverse && endReverse)) continue;
|
||||
}
|
||||
return (true, intraNode, ts);
|
||||
}
|
||||
return (false, null, null);
|
||||
}
|
||||
|
||||
private List<NodeDto> GetIntermediateNode(NodeDto startNode, NodeDto endNode)
|
||||
{
|
||||
var edge1s = Edges.Where(e => e.StartNodeId == startNode.Id || e.EndNodeId == startNode.Id).ToList();
|
||||
var edge2s = Edges.Where(e => e.StartNodeId == endNode.Id || e.EndNodeId == endNode.Id).ToList();
|
||||
if (edge1s is null || edge2s is null || edge1s.Count < 2 || edge2s.Count < 2) return [];
|
||||
List<NodeDto> node1 = [];
|
||||
List<NodeDto> IntermediateNode = [];
|
||||
foreach (var edge1 in edge1s)
|
||||
{
|
||||
if (edge1.TrajectoryDegree != TrajectoryDegree.One) continue;
|
||||
Guid interNodeId = Guid.Empty;
|
||||
if (edge1.StartNodeId == startNode.Id && (edge1.DirectionAllowed == DirectionAllowed.Both || edge1.DirectionAllowed == DirectionAllowed.Forward)) interNodeId = edge1.EndNodeId;
|
||||
else if (edge1.DirectionAllowed == DirectionAllowed.Both || edge1.DirectionAllowed == DirectionAllowed.Forward) interNodeId = edge1.StartNodeId;
|
||||
if (interNodeId == Guid.Empty || interNodeId == endNode.Id) continue;
|
||||
var interNode = Nodes.FirstOrDefault(n => n.Id == interNodeId);
|
||||
if (interNode is null) continue;
|
||||
//(double distance, double x, double y) = PathPlanning.DistanceToRangeSegment(interNode.X, interNode.Y, startNode.X, startNode.Y, endNode.X, endNode.Y);
|
||||
//if (distance < 0.3 && x != startNode.X && x != endNode.X && y != startNode.Y && y != endNode.Y)
|
||||
//{
|
||||
// node1.Add(interNode);
|
||||
//}
|
||||
node1.Add(interNode);
|
||||
}
|
||||
if (node1.Count == 0) return [];
|
||||
foreach (var edge2 in edge2s)
|
||||
{
|
||||
if (edge2.TrajectoryDegree != TrajectoryDegree.One) continue;
|
||||
Guid interNodeId = Guid.Empty;
|
||||
if (edge2.StartNodeId == endNode.Id && (edge2.DirectionAllowed == DirectionAllowed.Both || edge2.DirectionAllowed == DirectionAllowed.Forward)) interNodeId = edge2.EndNodeId;
|
||||
else if (edge2.DirectionAllowed == DirectionAllowed.Both || edge2.DirectionAllowed == DirectionAllowed.Forward) interNodeId = edge2.StartNodeId;
|
||||
if (interNodeId == Guid.Empty || interNodeId == startNode.Id) continue;
|
||||
var interNode = Nodes.FirstOrDefault(n => n.Id == interNodeId);
|
||||
if (interNode is null) continue;
|
||||
//(double distance, double x, double y) = PathPlanning.DistanceToRangeSegment(interNode.X, interNode.Y, startNode.X, startNode.Y, endNode.X, endNode.Y);
|
||||
//if (distance < 0.3 && x != startNode.X && x != endNode.X && y != startNode.Y && y != endNode.Y)
|
||||
//{
|
||||
// if (node1.Any(n => n.Id == interNode.Id) && !IntermediateNode.Any(n => n.Id == interNode.Id) && interNode.Id != startNode.Id)
|
||||
// IntermediateNode.Add(interNode);
|
||||
//}
|
||||
if (node1.Any(n => n.Id == interNode.Id) && !IntermediateNode.Any(n => n.Id == interNode.Id) && interNode.Id != startNode.Id)
|
||||
IntermediateNode.Add(interNode);
|
||||
}
|
||||
return IntermediateNode;
|
||||
}
|
||||
|
||||
private EdgeDto[] GetEdgesPlanning(NodeDto[] nodes)
|
||||
{
|
||||
var EdgesPlanning = new List<EdgeDto>();
|
||||
for (int i = 0; i < nodes.Length - 1; i++)
|
||||
{
|
||||
var edge = Edges.FirstOrDefault(e => e.StartNodeId == nodes[i].Id && e.EndNodeId == nodes[i + 1].Id ||
|
||||
e.EndNodeId == nodes[i].Id && e.StartNodeId == nodes[i + 1].Id);
|
||||
if (edge is null)
|
||||
{
|
||||
if (i != 0) return [];
|
||||
EdgesPlanning.Add(new EdgeDto()
|
||||
{
|
||||
Id = Guid.NewGuid(),
|
||||
StartNodeId = nodes[i].Id,
|
||||
EndNodeId = nodes[i + 1].Id,
|
||||
DirectionAllowed = DirectionAllowed.Both,
|
||||
TrajectoryDegree = TrajectoryDegree.One,
|
||||
});
|
||||
continue;
|
||||
}
|
||||
bool isReverse = nodes[i].Id != edge.StartNodeId && edge.TrajectoryDegree == TrajectoryDegree.Three; ;
|
||||
EdgesPlanning.Add(new()
|
||||
{
|
||||
Id = edge.Id,
|
||||
StartNodeId = nodes[i].Id,
|
||||
EndNodeId = nodes[i + 1].Id,
|
||||
DirectionAllowed = edge.DirectionAllowed,
|
||||
TrajectoryDegree = edge.TrajectoryDegree,
|
||||
ControlPoint1X = isReverse ? edge.ControlPoint2X : edge.ControlPoint1X,
|
||||
ControlPoint1Y = isReverse ? edge.ControlPoint2Y : edge.ControlPoint1Y,
|
||||
ControlPoint2X = isReverse ? edge.ControlPoint1X : edge.ControlPoint2X,
|
||||
ControlPoint2Y = isReverse ? edge.ControlPoint1Y : edge.ControlPoint2Y
|
||||
});
|
||||
}
|
||||
return [.. EdgesPlanning];
|
||||
}
|
||||
|
||||
private (NodeDto[] NodesFilter, EdgeDto[] EdgesFilter) FilterPathPlanning(NodeDto[] nodes, EdgeDto[] edges)
|
||||
{
|
||||
if (nodes.Length <= 1 || edges.Length < 1 || nodes.Length - 1 != edges.Length) return ([], []);
|
||||
List<NodeDto> nodeFilter = [nodes[0]];
|
||||
for (int i = 1; i < nodes.Length - 1; i++)
|
||||
{
|
||||
var IntermediateNode = GetIntermediateNode(nodes[i - 1], nodes[i]);
|
||||
if (IntermediateNode is null || IntermediateNode.Count == 0)
|
||||
{
|
||||
nodeFilter.Add(nodes[i]);
|
||||
continue;
|
||||
}
|
||||
if (IntermediateNode.Any(n => n.Id == nodes[i + 1].Id))
|
||||
{
|
||||
nodeFilter.Add(nodes[i + 1]);
|
||||
i++;
|
||||
}
|
||||
else nodeFilter.Add(nodes[i]);
|
||||
}
|
||||
nodeFilter.Add(nodes[^1]);
|
||||
var edgeFilter = GetEdgesPlanning([.. nodeFilter]);
|
||||
if (nodeFilter.Count - 1 != edgeFilter.Length) return ([], []);
|
||||
return ([.. nodeFilter], [.. edgeFilter]);
|
||||
}
|
||||
|
||||
private double GetLength(EdgeDto[] edges)
|
||||
{
|
||||
if (edges.Length == 0) return 999;
|
||||
double distance = 0;
|
||||
for (int i = 0; i < edges.Length; i++)
|
||||
{
|
||||
var edge = edges.FirstOrDefault(e => e.Id == edges[i].Id);
|
||||
if (edge is null) return 999;
|
||||
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 999;
|
||||
distance += MapEditorHelper.GetEdgeLength(new()
|
||||
{
|
||||
X1 = startNode.X,
|
||||
X2 = endNode.X,
|
||||
Y1 = startNode.Y,
|
||||
Y2 = endNode.Y,
|
||||
TrajectoryDegree = edge.TrajectoryDegree,
|
||||
ControlPoint1X = edge.ControlPoint1X,
|
||||
ControlPoint1Y = edge.ControlPoint1Y,
|
||||
ControlPoint2X = edge.ControlPoint2X,
|
||||
ControlPoint2Y = edge.ControlPoint2Y,
|
||||
});
|
||||
}
|
||||
return distance;
|
||||
}
|
||||
|
||||
private static NodeDto[] CalculatorDirection(NodeDto[] nodes, EdgeDto[] edges, RobotDirection currentDirection)
|
||||
{
|
||||
var FinalDirection = GetRobotDirectionInPath(currentDirection, nodes, edges, ReverseDirectionAngleDegree);
|
||||
NodeDto[] returnNodes = [.. nodes];
|
||||
for (var i = 0; i < returnNodes.Length; i++)
|
||||
{
|
||||
returnNodes[i].Direction = MapCompute.GetNodeDirection(FinalDirection[i]);
|
||||
}
|
||||
return returnNodes;
|
||||
}
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanning(double x, double y, double theta, Guid goalId, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
|
||||
if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
|
||||
|
||||
var AStarPathPlanner = new AStarPathPlanner(Nodes, Edges);
|
||||
var Path = AStarPathPlanner.Planning(x,
|
||||
y,
|
||||
new NodeDto() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
|
||||
Options.LimitDistanceToEdge,
|
||||
Options.LimitDistanceToNode,
|
||||
cancellationToken);
|
||||
if (!Path.IsSuccess) return new(false, Path.Message);
|
||||
if (Path.Data is null || Path.Data.Count < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{x}, {y}, {theta}]");
|
||||
if (Path.Data.Count == 1) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
|
||||
|
||||
var edgeplannings = GetEdgesPlanning([.. Path.Data]);
|
||||
|
||||
RobotDirection CurrenDirection = GetRobotDirection(Path.Data[0], Path.Data[1], edgeplannings[0], theta, RobotDirectionWithAngle, false);
|
||||
|
||||
return new(true)
|
||||
{
|
||||
Data = ([.. CalculatorDirection([.. Path.Data], [.. edgeplannings], CurrenDirection)], [.. edgeplannings])
|
||||
};
|
||||
}
|
||||
|
||||
private MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> CheckPathWithFinalDirection(NodeDto[] nodes, EdgeDto[] edges, double currentAngle, RobotDirection goalDirection = RobotDirection.NONE)
|
||||
{
|
||||
if ((nodes[^1].Direction == MapCompute.GetNodeDirection(goalDirection) && GetLength([.. edges]) < 10) || goalDirection == RobotDirection.NONE)
|
||||
return new(true) { Data = FilterPathPlanning([.. nodes], [.. edges]) };
|
||||
|
||||
var edgeplannings = edges.ToList();
|
||||
var nodeplannings = nodes.ToList();
|
||||
|
||||
var TStructures = GetTStructure();
|
||||
|
||||
Guid LastReverseDirectionId = Guid.Empty;
|
||||
NodeDto LastNodeReverseDirection = new();
|
||||
for (int i = 1; i < nodeplannings.Count; i++)
|
||||
{
|
||||
if (nodeplannings[i].Direction == Direction.FORWARD) continue;
|
||||
NodeDto? futureNode = null;
|
||||
EdgeDto? futureEdge = null;
|
||||
if (i < nodeplannings.Count - 1)
|
||||
{
|
||||
futureNode = nodeplannings[i + 1];
|
||||
futureEdge = edgeplannings[i];
|
||||
}
|
||||
double startAngle = currentAngle;
|
||||
if (i >= 2) startAngle = GetNodeAngle(nodeplannings[i - 1], nodeplannings[i - 2], edgeplannings[i - 2]);
|
||||
(var IsSuccess, var intraNode, var tstructure) = IsReverse(nodeplannings[i], nodeplannings[i - 1], futureNode, edgeplannings[i - 1], futureEdge, startAngle, TStructures);
|
||||
if (!IsSuccess || intraNode is null || tstructure is null) continue;
|
||||
var edge1 = Edges.FirstOrDefault(e => (e.StartNodeId == nodeplannings[i - 1].Id && e.EndNodeId == intraNode.Id) ||
|
||||
e.EndNodeId == nodeplannings[i - 1].Id && e.StartNodeId == intraNode.Id);
|
||||
var edge2 = Edges.FirstOrDefault(e => (e.StartNodeId == nodeplannings[i].Id && e.EndNodeId == intraNode.Id) ||
|
||||
e.EndNodeId == nodeplannings[i].Id && e.StartNodeId == intraNode.Id);
|
||||
if (edge1 is null || edge2 is null) continue;
|
||||
edgeplannings.RemoveAt(i - 1);
|
||||
edgeplannings.Insert(i - 1, new()
|
||||
{
|
||||
Id = edge1.Id,
|
||||
StartNodeId = nodeplannings[i - 1].Id,
|
||||
EndNodeId = intraNode.Id,
|
||||
TrajectoryDegree = edge1.TrajectoryDegree,
|
||||
DirectionAllowed = edge1.DirectionAllowed,
|
||||
ControlPoint1X = edge1.ControlPoint1X,
|
||||
ControlPoint1Y = edge1.ControlPoint1Y,
|
||||
ControlPoint2X = edge1.ControlPoint2X,
|
||||
ControlPoint2Y = edge1.ControlPoint2Y
|
||||
});
|
||||
edgeplannings.Insert(i, new()
|
||||
{
|
||||
Id = edge2.Id,
|
||||
StartNodeId = intraNode.Id,
|
||||
EndNodeId = nodeplannings[i].Id,
|
||||
TrajectoryDegree = edge2.TrajectoryDegree,
|
||||
DirectionAllowed = edge2.DirectionAllowed,
|
||||
ControlPoint1X = edge2.ControlPoint1X,
|
||||
ControlPoint1Y = edge2.ControlPoint1Y,
|
||||
ControlPoint2X = edge2.ControlPoint2X,
|
||||
ControlPoint2Y = edge2.ControlPoint2Y
|
||||
});
|
||||
nodeplannings.Insert(i, intraNode);
|
||||
var directionInPath = GetRobotDirectionInPath(MapCompute.GetRobotDirection(nodeplannings[0].Direction), [.. nodeplannings], [.. edgeplannings], ReverseDirectionAngleDegree);
|
||||
foreach (var node in nodeplannings)
|
||||
{
|
||||
node.Direction = MapCompute.GetNodeDirection(directionInPath[nodeplannings.IndexOf(node)]);
|
||||
}
|
||||
LastReverseDirectionId = tstructure.Id;
|
||||
LastNodeReverseDirection = nodeplannings[i + 1];
|
||||
i++;
|
||||
}
|
||||
|
||||
if (nodeplannings[^1].Direction == MapCompute.GetNodeDirection(goalDirection))
|
||||
return new(true) { Data = FilterPathPlanning([.. nodeplannings], [.. edgeplannings]) };
|
||||
|
||||
for (int i = nodeplannings.Count - 1; i > 0; i--)
|
||||
{
|
||||
NodeDto? futureNode = null;
|
||||
EdgeDto? futureEdge = null;
|
||||
if (i < nodeplannings.Count - 1)
|
||||
{
|
||||
futureNode = nodeplannings[i + 1];
|
||||
futureEdge = edgeplannings[i];
|
||||
}
|
||||
double startAngle = currentAngle;
|
||||
if (i >= 2) startAngle = GetNodeAngle(nodeplannings[i - 1], nodeplannings[i - 2], edgeplannings[i - 2]);
|
||||
(var IsSuccess, var intraNode, var tstructure) = IsReverse(nodeplannings[i], nodeplannings[i - 1], futureNode, edgeplannings[i - 1], futureEdge, startAngle, TStructures);
|
||||
if (!IsSuccess || intraNode is null || tstructure is null) continue;
|
||||
|
||||
if (nodeplannings[i - 1].Id == LastNodeReverseDirection.Id)
|
||||
{
|
||||
var edge = Edges.FirstOrDefault(e => (e.StartNodeId == nodeplannings[i - 2].Id && e.EndNodeId == nodeplannings[i].Id) ||
|
||||
(e.StartNodeId == nodeplannings[i].Id && e.EndNodeId == nodeplannings[i - 2].Id));
|
||||
if (edge is null) continue;
|
||||
edgeplannings.Insert(i - 2, new()
|
||||
{
|
||||
Id = edge.Id,
|
||||
StartNodeId = nodeplannings[i - 2].Id,
|
||||
EndNodeId = nodeplannings[i].Id,
|
||||
TrajectoryDegree = edge.TrajectoryDegree,
|
||||
DirectionAllowed = edge.DirectionAllowed,
|
||||
ControlPoint1X = edge.ControlPoint1X,
|
||||
ControlPoint1Y = edge.ControlPoint1Y,
|
||||
ControlPoint2X = edge.ControlPoint2X,
|
||||
ControlPoint2Y = edge.ControlPoint2Y
|
||||
});
|
||||
edgeplannings.RemoveAt(i);
|
||||
edgeplannings.RemoveAt(i - 1);
|
||||
nodeplannings.RemoveAt(i - 1);
|
||||
}
|
||||
else if (tstructure.Id != LastReverseDirectionId || i < 2)
|
||||
{
|
||||
var edge1 = Edges.FirstOrDefault(e => (e.StartNodeId == nodeplannings[i - 1].Id && e.EndNodeId == intraNode.Id) ||
|
||||
e.EndNodeId == nodeplannings[i - 1].Id && e.StartNodeId == intraNode.Id);
|
||||
var edge2 = Edges.FirstOrDefault(e => (e.StartNodeId == nodeplannings[i].Id && e.EndNodeId == intraNode.Id) ||
|
||||
e.EndNodeId == nodeplannings[i].Id && e.StartNodeId == intraNode.Id);
|
||||
if (edge1 is null || edge2 is null) continue;
|
||||
edgeplannings.RemoveAt(i - 1);
|
||||
edgeplannings.Insert(i - 1, new()
|
||||
{
|
||||
Id = edge1.Id,
|
||||
StartNodeId = nodeplannings[i - 1].Id,
|
||||
EndNodeId = intraNode.Id,
|
||||
TrajectoryDegree = edge1.TrajectoryDegree,
|
||||
DirectionAllowed = edge1.DirectionAllowed,
|
||||
ControlPoint1X = edge1.ControlPoint1X,
|
||||
ControlPoint1Y = edge1.ControlPoint1Y,
|
||||
ControlPoint2X = edge1.ControlPoint2X,
|
||||
ControlPoint2Y = edge1.ControlPoint2Y,
|
||||
});
|
||||
edgeplannings.Insert(i, new()
|
||||
{
|
||||
Id = edge2.Id,
|
||||
StartNodeId = intraNode.Id,
|
||||
EndNodeId = nodeplannings[i].Id,
|
||||
TrajectoryDegree = edge2.TrajectoryDegree,
|
||||
DirectionAllowed = edge2.DirectionAllowed,
|
||||
ControlPoint1X = edge2.ControlPoint1X,
|
||||
ControlPoint1Y = edge2.ControlPoint1Y,
|
||||
ControlPoint2X = edge2.ControlPoint2X,
|
||||
ControlPoint2Y = edge2.ControlPoint2Y,
|
||||
});
|
||||
nodeplannings.Insert(i, intraNode);
|
||||
}
|
||||
else
|
||||
{
|
||||
var edge = Edges.FirstOrDefault(e => (e.StartNodeId == nodeplannings[i - 2].Id && e.EndNodeId == nodeplannings[i].Id) ||
|
||||
(e.StartNodeId == nodeplannings[i].Id && e.EndNodeId == nodeplannings[i - 2].Id));
|
||||
if (edge is null) continue;
|
||||
edgeplannings.Insert(i - 2, new()
|
||||
{
|
||||
Id = edge.Id,
|
||||
StartNodeId = nodeplannings[i - 2].Id,
|
||||
EndNodeId = nodeplannings[i].Id,
|
||||
TrajectoryDegree = edge.TrajectoryDegree,
|
||||
DirectionAllowed = edge.DirectionAllowed,
|
||||
ControlPoint1X = edge.ControlPoint1X,
|
||||
ControlPoint1Y = edge.ControlPoint1Y,
|
||||
ControlPoint2X = edge.ControlPoint2X,
|
||||
ControlPoint2Y = edge.ControlPoint2Y,
|
||||
});
|
||||
edgeplannings.RemoveAt(i);
|
||||
edgeplannings.RemoveAt(i - 1);
|
||||
nodeplannings.RemoveAt(i - 1);
|
||||
}
|
||||
return new(true) { Data = FilterPathPlanning([.. CalculatorDirection([.. nodeplannings], [.. edgeplannings], MapCompute.GetRobotDirection(nodeplannings[0].Direction))], [.. edgeplannings]) };
|
||||
}
|
||||
return new(false, $"Đường đi đến đích không thỏa mãn điều kiện");
|
||||
}
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithAngle(double x, double y, double theta, Guid goalId, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
|
||||
if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
|
||||
|
||||
var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
|
||||
if (!basicPath.IsSuccess) return basicPath;
|
||||
if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
|
||||
|
||||
RobotDirection goalDirection = GetRobotDirection(basicPath.Data.Nodes[^1], basicPath.Data.Nodes[^2], basicPath.Data.Edges[^1], goal.Theta, RobotDirectionWithAngle, true);
|
||||
|
||||
return CheckPathWithFinalDirection(basicPath.Data.Nodes, basicPath.Data.Edges, theta, goalDirection);
|
||||
}
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithStartDirection(double x, double y, double theta, Guid goalId, RobotDirection startDiretion = RobotDirection.NONE, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
throw new NotImplementedException();
|
||||
}
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithFinalDirection(double x, double y, double theta, Guid goalId, RobotDirection goalDirection = RobotDirection.NONE, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
|
||||
if (!basicPath.IsSuccess) return basicPath;
|
||||
if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
|
||||
|
||||
return CheckPathWithFinalDirection(basicPath.Data.Nodes, basicPath.Data.Edges, theta, goalDirection);
|
||||
}
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanning(Guid startNodeId, double theta, Guid goalId, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
|
||||
if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
|
||||
|
||||
var startNode = Nodes.FirstOrDefault(n => n.Id == startNodeId);
|
||||
if (startNode is null) return new(false, $"Điểm bắt đầu {startNodeId} không tồn tại trong map");
|
||||
|
||||
var AStarPathPlanner = new AStarPathPlanner(Nodes, Edges);
|
||||
var Path = AStarPathPlanner.Planning(startNode, goal, cancellationToken);
|
||||
if (!Path.IsSuccess) return new(false, Path.Message);
|
||||
if (Path.Data is null || Path.Data.Length < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{startNode.Name} - {startNode.Id}]");
|
||||
if (Path.Data.Length == 1) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
|
||||
|
||||
var edgeplannings = GetEdgesPlanning([.. Path.Data]);
|
||||
|
||||
RobotDirection CurrenDirection = GetRobotDirection(Path.Data[0], Path.Data[1], edgeplannings[0], theta, RobotDirectionWithAngle, false);
|
||||
|
||||
return new(true)
|
||||
{
|
||||
Data = ([.. CalculatorDirection([.. Path.Data], [.. edgeplannings], CurrenDirection)], [.. edgeplannings])
|
||||
};
|
||||
}
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithAngle(Guid startNodeId, double theta, Guid goalId, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
|
||||
if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
|
||||
|
||||
var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
|
||||
if (!basicPath.IsSuccess) return basicPath;
|
||||
if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
|
||||
|
||||
RobotDirection goalDirection = GetRobotDirection(basicPath.Data.Nodes[^1], basicPath.Data.Nodes[^2], basicPath.Data.Edges[^1], goal.Theta, RobotDirectionWithAngle, true);
|
||||
|
||||
return CheckPathWithFinalDirection(basicPath.Data.Nodes, basicPath.Data.Edges, theta, goalDirection);
|
||||
}
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithStartDirection(Guid startNodeId, double theta, Guid goalId, RobotDirection startDiretion = RobotDirection.NONE, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
throw new NotImplementedException();
|
||||
}
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithFinalDirection(Guid startNodeId, double theta, Guid goalId, RobotDirection goalDirection = RobotDirection.NONE, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
|
||||
if (!basicPath.IsSuccess) return basicPath;
|
||||
if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
|
||||
|
||||
return CheckPathWithFinalDirection(basicPath.Data.Nodes, basicPath.Data.Edges, theta, goalDirection);
|
||||
}
|
||||
}
|
||||
135
RobotNet.RobotManager/Services/Planner/Fokrlift/TStructure.cs
Normal file
135
RobotNet.RobotManager/Services/Planner/Fokrlift/TStructure.cs
Normal file
@@ -0,0 +1,135 @@
|
||||
using RobotNet.MapShares;
|
||||
using RobotNet.MapShares.Dtos;
|
||||
using RobotNet.MapShares.Enums;
|
||||
|
||||
namespace RobotNet.RobotManager.Services.Planner.Fokrlift;
|
||||
public enum TStructureDirection
|
||||
{
|
||||
NODE1_NODE2_NODE3,
|
||||
NODE1_NODE3_NODE2,
|
||||
NODE2_NODE1_NODE3,
|
||||
NODE2_NODE3_NODE1,
|
||||
NODE3_NODE2_NODE1,
|
||||
NODE3_NODE1_NODE2,
|
||||
}
|
||||
|
||||
public class TStructure
|
||||
{
|
||||
public Guid Id { get; set; } = Guid.NewGuid();
|
||||
public NodeDto Node1 { get; set; } = new();
|
||||
public NodeDto Node2 { get; set; } = new();
|
||||
public NodeDto Node3 { get; set; } = new();
|
||||
public EdgeDto Edge12 { get; set; } = new();
|
||||
public EdgeDto Edge13 { get; set; } = new();
|
||||
public EdgeDto Edge23 { get; set; } = new();
|
||||
private const double Ratio = 0.1;
|
||||
|
||||
public bool IsDriectionReverse(TStructureDirection direction)
|
||||
{
|
||||
NodeDto OriginNode = new();
|
||||
NodeDto ToWardNode1 = new();
|
||||
NodeDto ToWardNode2 = new();
|
||||
EdgeDto ToWardEdge1 = new();
|
||||
EdgeDto ToWardEdge2 = new();
|
||||
|
||||
switch (direction)
|
||||
{
|
||||
case TStructureDirection.NODE3_NODE2_NODE1:
|
||||
case TStructureDirection.NODE1_NODE2_NODE3:
|
||||
OriginNode = Node2;
|
||||
ToWardNode1 = Node1;
|
||||
ToWardNode2 = Node3;
|
||||
ToWardEdge1 = Edge12;
|
||||
ToWardEdge2 = Edge23;
|
||||
break;
|
||||
case TStructureDirection.NODE2_NODE3_NODE1:
|
||||
case TStructureDirection.NODE1_NODE3_NODE2:
|
||||
OriginNode = Node3;
|
||||
ToWardNode1 = Node1;
|
||||
ToWardNode2 = Node2;
|
||||
ToWardEdge1 = Edge13;
|
||||
ToWardEdge2 = Edge23;
|
||||
break;
|
||||
case TStructureDirection.NODE3_NODE1_NODE2:
|
||||
case TStructureDirection.NODE2_NODE1_NODE3:
|
||||
OriginNode = Node1;
|
||||
ToWardNode1 = Node2;
|
||||
ToWardNode2 = Node3;
|
||||
ToWardEdge1 = Edge12;
|
||||
ToWardEdge2 = Edge13;
|
||||
break;
|
||||
}
|
||||
|
||||
var NearToWardNode1 = MapEditorHelper.GetNearByNode(OriginNode, ToWardNode1, ToWardEdge1, Ratio);
|
||||
var NearToWardNode3 = MapEditorHelper.GetNearByNode(OriginNode, ToWardNode2, ToWardEdge2, Ratio);
|
||||
var angle = MapEditorHelper.GetAngle(OriginNode, NearToWardNode1, NearToWardNode3);
|
||||
if (angle < 50) return true;
|
||||
return false;
|
||||
}
|
||||
|
||||
public bool IsDriectionReverse(NodeDto node1, NodeDto node2)
|
||||
{
|
||||
if (node1.Id == Node1.Id)
|
||||
{
|
||||
if (node2.Id == Node2.Id) return IsDriectionReverse(TStructureDirection.NODE1_NODE3_NODE2);
|
||||
else if (node2.Id == Node3.Id) return IsDriectionReverse(TStructureDirection.NODE1_NODE2_NODE3);
|
||||
}
|
||||
else if (node1.Id == Node2.Id)
|
||||
{
|
||||
if (node2.Id == Node1.Id) return IsDriectionReverse(TStructureDirection.NODE2_NODE3_NODE1);
|
||||
else if (node2.Id == Node3.Id) return IsDriectionReverse(TStructureDirection.NODE2_NODE1_NODE3);
|
||||
}
|
||||
else if (node1.Id == Node3.Id)
|
||||
{
|
||||
if (node2.Id == Node1.Id) return IsDriectionReverse(TStructureDirection.NODE3_NODE2_NODE1);
|
||||
else if (node2.Id == Node2.Id) return IsDriectionReverse(TStructureDirection.NODE3_NODE1_NODE2);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
public NodeDto? GetIntraNode(NodeDto node1, NodeDto node2)
|
||||
{
|
||||
if (node1.Id == Node1.Id)
|
||||
{
|
||||
if (node2.Id == Node2.Id) return Node3;
|
||||
else if (node2.Id == Node3.Id) return Node2;
|
||||
}
|
||||
else if (node1.Id == Node2.Id)
|
||||
{
|
||||
if (node2.Id == Node1.Id) return Node3;
|
||||
else if (node2.Id == Node3.Id) return Node1;
|
||||
}
|
||||
else if (node1.Id == Node3.Id)
|
||||
{
|
||||
if (node2.Id == Node1.Id) return Node2;
|
||||
else if (node2.Id == Node2.Id) return Node1;
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
public EdgeDto? GetEdge(NodeDto node1, NodeDto node2)
|
||||
{
|
||||
if (Edge12.StartNodeId == node1.Id || Edge12.EndNodeId == node1.Id)
|
||||
{
|
||||
if (Edge12.StartNodeId == node2.Id || Edge12.EndNodeId == node2.Id) return Edge12;
|
||||
}
|
||||
if (Edge13.StartNodeId == node1.Id || Edge13.EndNodeId == node1.Id)
|
||||
{
|
||||
if (Edge13.StartNodeId == node2.Id || Edge13.EndNodeId == node2.Id) return Edge13;
|
||||
}
|
||||
if (Edge23.StartNodeId == node1.Id || Edge23.EndNodeId == node1.Id)
|
||||
{
|
||||
if (Edge23.StartNodeId == node2.Id || Edge23.EndNodeId == node2.Id) return Edge23;
|
||||
}
|
||||
return null;
|
||||
}
|
||||
|
||||
public bool IsAccessDirection(NodeDto startNode, NodeDto endNode)
|
||||
{
|
||||
var edge = GetEdge(startNode, endNode);
|
||||
if (edge is null) return false;
|
||||
if (edge.StartNodeId == startNode.Id && (edge.DirectionAllowed == DirectionAllowed.Both || edge.DirectionAllowed == DirectionAllowed.Forward)) return true;
|
||||
if (edge.EndNodeId == startNode.Id && (edge.DirectionAllowed == DirectionAllowed.Both || edge.DirectionAllowed == DirectionAllowed.Backward)) return true;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,310 @@
|
||||
using RobotNet.MapShares;
|
||||
using RobotNet.MapShares.Dtos;
|
||||
using RobotNet.MapShares.Enums;
|
||||
using RobotNet.RobotManager.Services.Planner.AStar;
|
||||
using RobotNet.RobotManager.Services.Planner.Fokrlift;
|
||||
using RobotNet.RobotManager.Services.Planner.Space;
|
||||
using RobotNet.RobotShares.Enums;
|
||||
using RobotNet.RobotShares.Models;
|
||||
using RobotNet.Shares;
|
||||
|
||||
namespace RobotNet.RobotManager.Services.Planner.ForkliftV2;
|
||||
|
||||
public class ForkLiftPathPlannerV2 : IPathPlanner
|
||||
{
|
||||
private List<NodeDto> Nodes = [];
|
||||
private List<EdgeDto> Edges = [];
|
||||
private const double ReverseDirectionAngleDegree = 80;
|
||||
private const double RobotDirectionWithAngle = 90;
|
||||
private const double Ratio = 0.1;
|
||||
|
||||
private readonly PathPlanningOptions Options = new()
|
||||
{
|
||||
LimitDistanceToEdge = 1,
|
||||
LimitDistanceToNode = 0.3,
|
||||
ResolutionSplit = 0.1,
|
||||
};
|
||||
|
||||
public void SetData(NodeDto[] nodes, EdgeDto[] edges)
|
||||
{
|
||||
Nodes = [.. nodes];
|
||||
Edges = [.. edges];
|
||||
}
|
||||
|
||||
public void SetOptions(PathPlanningOptions options)
|
||||
{
|
||||
Options.LimitDistanceToNode = options.LimitDistanceToNode;
|
||||
Options.LimitDistanceToEdge = options.LimitDistanceToEdge;
|
||||
Options.ResolutionSplit = options.ResolutionSplit;
|
||||
}
|
||||
|
||||
public static RobotDirection[] GetRobotDirectionInPath(RobotDirection currentDirection, List<NodeDto> nodeplanning, List<EdgeDto> edgePlanning)
|
||||
{
|
||||
RobotDirection[] RobotDirectionInNode = new RobotDirection[nodeplanning.Count];
|
||||
if (nodeplanning.Count > 0) RobotDirectionInNode[0] = currentDirection;
|
||||
if (nodeplanning.Count > 2)
|
||||
{
|
||||
for (int i = 1; i < nodeplanning.Count - 1; i++)
|
||||
{
|
||||
NodeDto startNode = MapEditorHelper.GetNearByNode(nodeplanning[i], nodeplanning[i - 1], edgePlanning[i - 1], Ratio);
|
||||
NodeDto endNode = MapEditorHelper.GetNearByNode(nodeplanning[i], nodeplanning[i + 1], edgePlanning[i], Ratio); ;
|
||||
var angle = MapEditorHelper.GetAngle(nodeplanning[i], startNode, endNode);
|
||||
if (angle < ReverseDirectionAngleDegree) RobotDirectionInNode[i] = RobotDirectionInNode[i - 1] == RobotDirection.FORWARD ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
|
||||
else RobotDirectionInNode[i] = RobotDirectionInNode[i - 1];
|
||||
}
|
||||
}
|
||||
if (nodeplanning.Count > 1) RobotDirectionInNode[^1] = RobotDirectionInNode[^2];
|
||||
return RobotDirectionInNode;
|
||||
}
|
||||
|
||||
public static RobotDirection GetRobotDirection(NodeDto currentNode, NodeDto nearNode, EdgeDto edge, double robotInNodeAngle, bool isReverse)
|
||||
{
|
||||
NodeDto NearNode = MapEditorHelper.GetNearByNode(currentNode, nearNode, edge, Ratio);
|
||||
|
||||
var RobotNearNode = new NodeDto()
|
||||
{
|
||||
X = currentNode.X + Math.Cos(robotInNodeAngle * Math.PI / 180),
|
||||
Y = currentNode.Y + Math.Sin(robotInNodeAngle * Math.PI / 180),
|
||||
};
|
||||
var angle = MapEditorHelper.GetAngle(currentNode, NearNode, RobotNearNode);
|
||||
if (angle > ReverseDirectionAngleDegree) return isReverse ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
|
||||
else return isReverse ? RobotDirection.FORWARD : RobotDirection.BACKWARD;
|
||||
}
|
||||
|
||||
private static NodeDto[] CalculatorDirection(List<NodeDto> nodes, List<EdgeDto> edges, RobotDirection currentDirection)
|
||||
{
|
||||
var FinalDirection = GetRobotDirectionInPath(currentDirection, nodes, edges);
|
||||
NodeDto[] returnNodes = [.. nodes];
|
||||
foreach (var item in returnNodes)
|
||||
{
|
||||
item.Direction = MapCompute.GetNodeDirection(FinalDirection[nodes.IndexOf(item)]);
|
||||
}
|
||||
return returnNodes;
|
||||
}
|
||||
|
||||
private List<EdgeDto> GetEdgesPlanning(List<NodeDto> nodes)
|
||||
{
|
||||
var EdgesPlanning = new List<EdgeDto>();
|
||||
for (int i = 0; i < nodes.Count - 1; i++)
|
||||
{
|
||||
var edge = Edges.FirstOrDefault(e => e.StartNodeId == nodes[i].Id && e.EndNodeId == nodes[i + 1].Id ||
|
||||
e.EndNodeId == nodes[i].Id && e.StartNodeId == nodes[i + 1].Id);
|
||||
if (edge is null)
|
||||
{
|
||||
if (i != 0) return [];
|
||||
EdgesPlanning.Add(new EdgeDto()
|
||||
{
|
||||
Id = Guid.NewGuid(),
|
||||
StartNodeId = nodes[i].Id,
|
||||
EndNodeId = nodes[i + 1].Id,
|
||||
DirectionAllowed = DirectionAllowed.Both,
|
||||
TrajectoryDegree = TrajectoryDegree.One,
|
||||
});
|
||||
continue;
|
||||
}
|
||||
bool isReverse = nodes[i].Id != edge.StartNodeId && edge.TrajectoryDegree == TrajectoryDegree.Three; ;
|
||||
EdgesPlanning.Add(new()
|
||||
{
|
||||
Id = edge.Id,
|
||||
StartNodeId = nodes[i].Id,
|
||||
EndNodeId = nodes[i + 1].Id,
|
||||
DirectionAllowed = edge.DirectionAllowed,
|
||||
TrajectoryDegree = edge.TrajectoryDegree,
|
||||
ControlPoint1X = isReverse ? edge.ControlPoint2X : edge.ControlPoint1X,
|
||||
ControlPoint1Y = isReverse ? edge.ControlPoint2Y : edge.ControlPoint1Y,
|
||||
ControlPoint2X = isReverse ? edge.ControlPoint1X : edge.ControlPoint2X,
|
||||
ControlPoint2Y = isReverse ? edge.ControlPoint1Y : edge.ControlPoint2Y
|
||||
});
|
||||
}
|
||||
return EdgesPlanning;
|
||||
}
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanning(double x, double y, double theta, Guid goalId, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
|
||||
if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
|
||||
|
||||
var AStarPathPlanner = new AStarPathPlanner(Nodes, Edges);
|
||||
var Path = AStarPathPlanner.Planning(x,
|
||||
y,
|
||||
new NodeDto() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
|
||||
Options.LimitDistanceToEdge,
|
||||
Options.LimitDistanceToNode,
|
||||
cancellationToken);
|
||||
if (!Path.IsSuccess) return new(false, Path.Message);
|
||||
if (Path.Data is null || Path.Data.Count < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{x}, {y}, {theta}]") { Data = ([goal], []) };
|
||||
if (Path.Data.Count == 1) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
|
||||
|
||||
var edgeplannings = GetEdgesPlanning([.. Path.Data]);
|
||||
return new(true) { Data = ([.. CalculatorDirection(Path.Data, edgeplannings, MapCompute.GetRobotDirection(Path.Data[0].Direction))], [.. edgeplannings]), };
|
||||
}
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithAngle(double x, double y, double theta, Guid goalId, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
|
||||
if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
|
||||
|
||||
var SSEAStarPathPlanner = new SSEAStarPathPlanner(Nodes, Edges);
|
||||
var Path = SSEAStarPathPlanner.PlanningWithGoalAngle(x,
|
||||
y,
|
||||
theta,
|
||||
new NodeDto() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y, Theta = goal.Theta },
|
||||
Options.LimitDistanceToEdge,
|
||||
Options.LimitDistanceToNode,
|
||||
cancellationToken);
|
||||
if (!Path.IsSuccess|| Path.Data is null || Path.Data.Count < 1)
|
||||
{
|
||||
var ForkliftV1 = new ForkliftPathPlanner();
|
||||
ForkliftV1.SetData([.. Nodes], [.. Edges]);
|
||||
return ForkliftV1.PathPlanningWithAngle(x,
|
||||
y,
|
||||
theta,
|
||||
goal.Id,
|
||||
cancellationToken);
|
||||
}
|
||||
else
|
||||
{
|
||||
if (Path.Data.Count == 1) return new(true, $"Robot đang đứng tại điểm đích [{goal.X}, {goal.Y}], robot: [{x}, {y}, {theta}]") { Data = ([goal], [])};
|
||||
|
||||
var edgeplannings = GetEdgesPlanning(Path.Data);
|
||||
if (edgeplannings.Count < 1) return new(false, $"Không thể lấy ra các đoạn thuộc tuyến đường đã tìm ra");
|
||||
|
||||
return new(true) { Data = ([.. CalculatorDirection(Path.Data, edgeplannings, MapCompute.GetRobotDirection(Path.Data[0].Direction))], [.. edgeplannings]), };
|
||||
}
|
||||
}
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithStartDirection(double x, double y, double theta, Guid goalId, RobotDirection startDiretion = RobotDirection.NONE, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
throw new NotImplementedException();
|
||||
}
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithFinalDirection(double x, double y, double theta, Guid goalId, RobotDirection goalDirection = RobotDirection.NONE, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
|
||||
if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
|
||||
|
||||
var SSEAStarPathPlanner = new SSEAStarPathPlanner(Nodes, Edges);
|
||||
var Path = SSEAStarPathPlanner.PlanningWithFinalDirection(x,
|
||||
y,
|
||||
theta,
|
||||
new NodeDto() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
|
||||
goalDirection,
|
||||
Options.LimitDistanceToEdge,
|
||||
Options.LimitDistanceToNode,
|
||||
cancellationToken);
|
||||
if (!Path.IsSuccess || Path.Data is null || Path.Data.Count < 1)
|
||||
{
|
||||
var ForkliftV1 = new ForkliftPathPlanner();
|
||||
ForkliftV1.SetData([.. Nodes], [.. Edges]);
|
||||
return ForkliftV1.PathPlanningWithFinalDirection(x,
|
||||
y,
|
||||
theta,
|
||||
goal.Id,
|
||||
goalDirection,
|
||||
cancellationToken);
|
||||
}
|
||||
else
|
||||
{
|
||||
if (Path.Data.Count == 1) return new(true, $"Robot đang đứng tại điểm đích [{goal.X}, {goal.Y}], robot: [{x}, {y}, {theta}]") { Data = ([goal], []) };
|
||||
|
||||
var edgeplannings = GetEdgesPlanning(Path.Data);
|
||||
if (edgeplannings.Count < 1) return new(false, $"Không thể lấy ra các đoạn thuộc tuyến đường đã tìm ra");
|
||||
|
||||
return new(true) { Data = ([.. CalculatorDirection(Path.Data, edgeplannings, MapCompute.GetRobotDirection(Path.Data[0].Direction))], [.. edgeplannings]), };
|
||||
}
|
||||
}
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanning(Guid startNodeId, double theta, Guid goalId, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
|
||||
if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
|
||||
|
||||
var startNode = Nodes.FirstOrDefault(n => n.Id == startNodeId);
|
||||
if (startNode is null) return new(false, $"Điểm bắt đầu {startNodeId} không tồn tại trong map");
|
||||
|
||||
var AStarPathPlanner = new AStarPathPlanner(Nodes, Edges);
|
||||
var Path = AStarPathPlanner.Planning(startNode,
|
||||
goal,
|
||||
cancellationToken);
|
||||
if (!Path.IsSuccess) return new(false, Path.Message);
|
||||
if (Path.Data is null || Path.Data.Length < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{startNode.Name} - {startNode.Id}]") { Data = ([goal], []) };
|
||||
if (Path.Data.Length == 1) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
|
||||
|
||||
var edgeplannings = GetEdgesPlanning([.. Path.Data]);
|
||||
|
||||
return new(true) { Data = ([.. CalculatorDirection([..Path.Data], edgeplannings, MapCompute.GetRobotDirection(Path.Data[0].Direction))], [.. edgeplannings]), };
|
||||
}
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithAngle(Guid startNodeId, double theta, Guid goalId, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
|
||||
if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
|
||||
|
||||
var startNode = Nodes.FirstOrDefault(n => n.Id == startNodeId);
|
||||
if (startNode is null) return new(false, $"Điểm bắt đầu {startNodeId} không tồn tại trong map");
|
||||
|
||||
var SSEAStarPathPlanner = new SSEAStarPathPlanner(Nodes, Edges);
|
||||
var Path = SSEAStarPathPlanner.PlanningWithGoalAngle(startNode,
|
||||
theta,
|
||||
goal,
|
||||
cancellationToken);
|
||||
if (!Path.IsSuccess || Path.Data is null || Path.Data.Length < 1)
|
||||
{
|
||||
var ForkliftV1 = new ForkliftPathPlanner();
|
||||
ForkliftV1.SetData([.. Nodes], [.. Edges]);
|
||||
return ForkliftV1.PathPlanningWithAngle(startNodeId,
|
||||
theta,
|
||||
goal.Id,
|
||||
cancellationToken);
|
||||
}
|
||||
else
|
||||
{
|
||||
if (Path.Data.Length == 1) return new(true, $"Robot đang đứng tại điểm đích [{goal.X}, {goal.Y}], robot: [{startNode.Name} - {startNode.Id}]") { Data = ([goal], []) };
|
||||
|
||||
var edgeplannings = GetEdgesPlanning([.. Path.Data]);
|
||||
if (edgeplannings.Count < 1) return new(false, $"Không thể lấy ra các đoạn thuộc tuyến đường đã tìm ra");
|
||||
|
||||
return new(true) { Data = ([.. CalculatorDirection([.. Path.Data], edgeplannings, MapCompute.GetRobotDirection(Path.Data[0].Direction))], [.. edgeplannings]), };
|
||||
}
|
||||
}
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithStartDirection(Guid startNodeId, double theta, Guid goalId, RobotDirection startDiretion = RobotDirection.NONE, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
throw new NotImplementedException();
|
||||
}
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithFinalDirection(Guid startNodeId, double theta, Guid goalId, RobotDirection goalDirection = RobotDirection.NONE, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
|
||||
if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
|
||||
|
||||
var startNode = Nodes.FirstOrDefault(n => n.Id == startNodeId);
|
||||
if (startNode is null) return new(false, $"Điểm bắt đầu {startNodeId} không tồn tại trong map");
|
||||
|
||||
var SSEAStarPathPlanner = new SSEAStarPathPlanner(Nodes, Edges);
|
||||
var Path = SSEAStarPathPlanner.PlanningWithFinalDirection(startNode,
|
||||
theta,
|
||||
goal,
|
||||
goalDirection,
|
||||
cancellationToken);
|
||||
if (!Path.IsSuccess || Path.Data is null || Path.Data.Length < 1)
|
||||
{
|
||||
var ForkliftV1 = new ForkliftPathPlanner();
|
||||
ForkliftV1.SetData([.. Nodes], [.. Edges]);
|
||||
return ForkliftV1.PathPlanningWithFinalDirection(startNodeId,
|
||||
theta,
|
||||
goal.Id,
|
||||
goalDirection,
|
||||
cancellationToken);
|
||||
}
|
||||
else
|
||||
{
|
||||
if (Path.Data.Length == 1) return new(true, $"Robot đang đứng tại điểm đích [{goal.X}, {goal.Y}], robot: [{startNode.Name} - {startNode.Id}]") { Data = ([goal], []) };
|
||||
|
||||
var edgeplannings = GetEdgesPlanning([.. Path.Data]);
|
||||
if (edgeplannings.Count < 1) return new(false, $"Không thể lấy ra các đoạn thuộc tuyến đường đã tìm ra");
|
||||
|
||||
return new(true) { Data = ([.. CalculatorDirection([.. Path.Data], edgeplannings, MapCompute.GetRobotDirection(Path.Data[0].Direction))], [.. edgeplannings]), };
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,31 @@
|
||||
using RobotNet.RobotShares.Enums;
|
||||
|
||||
namespace RobotNet.RobotManager.Services.Planner.ForkliftV2;
|
||||
|
||||
#nullable disable
|
||||
|
||||
public class SSEAStarNode
|
||||
{
|
||||
public Guid Id { get; set; }
|
||||
public double X { get; set; }
|
||||
public double Y { get; set; }
|
||||
public RobotDirection Direction { get; set; }
|
||||
public double Cost { get; set; }
|
||||
public double Heuristic { get; set; }
|
||||
public double TotalCost => Cost + Heuristic;
|
||||
public string Name { get; set; }
|
||||
public SSEAStarNode Parent { get; set; }
|
||||
public List<SSEAStarNode> NegativeNodes { get; set; } = [];
|
||||
|
||||
public override bool Equals(object obj)
|
||||
{
|
||||
if (obj is SSEAStarNode other)
|
||||
return Id == other.Id && Direction == other.Direction;
|
||||
return false;
|
||||
}
|
||||
|
||||
public override int GetHashCode()
|
||||
{
|
||||
return HashCode.Combine(Id, Direction);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,621 @@
|
||||
using RobotNet.MapShares;
|
||||
using RobotNet.MapShares.Dtos;
|
||||
using RobotNet.MapShares.Enums;
|
||||
using RobotNet.RobotManager.Services.Planner.Space;
|
||||
using RobotNet.RobotShares.Enums;
|
||||
using RobotNet.Shares;
|
||||
|
||||
namespace RobotNet.RobotManager.Services.Planner.ForkliftV2;
|
||||
|
||||
public class SSEAStarPathPlanner(List<NodeDto> Nodes, List<EdgeDto> Edges)
|
||||
{
|
||||
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);
|
||||
}
|
||||
|
||||
//public EdgeDto? GetClosesEdge(double x, double y, double limitDistance, CancellationToken? cancellationToken = null)
|
||||
//{
|
||||
// if (cancellationToken?.IsCancellationRequested == true) return null;
|
||||
// RTree RTree = new(Nodes, Edges);
|
||||
// return RTree.FindNearest(x, y, limitDistance);
|
||||
//}
|
||||
|
||||
public EdgeDto? GetClosesEdge(double x, double y, 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 = MapCompute.DistanceToEdge(x, y, edge, startNode, endNode);
|
||||
if (getDistance.IsSuccess)
|
||||
{
|
||||
if (getDistance.Data < minDistance)
|
||||
{
|
||||
minDistance = getDistance.Data;
|
||||
edgeResult = edge;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (minDistance <= limitDistance) return edgeResult;
|
||||
else return null;
|
||||
}
|
||||
|
||||
public 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.HasValue && 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 != 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 != null) listNodesNegative.Add(nodeAdd);
|
||||
continue;
|
||||
}
|
||||
}
|
||||
return listNodesNegative;
|
||||
}
|
||||
|
||||
private double GetNegativeCost(SSEAStarNode currentNode, SSEAStarNode negativeNode)
|
||||
{
|
||||
var negativeEdges = Edges.Where(e => e.StartNodeId == currentNode.Id && e.EndNodeId == negativeNode.Id ||
|
||||
e.StartNodeId == negativeNode.Id && e.EndNodeId == currentNode.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<SSEAStarNode> GetNegativeAStarNode(SSEAStarNode nodeCurrent, NodeDto endNode, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var possiblePointNegative = new List<SSEAStarNode>();
|
||||
if (nodeCurrent.Id == endNode.Id) return possiblePointNegative;
|
||||
|
||||
var listNodesNegative = GetNegativeNode(nodeCurrent.Id, cancellationToken);
|
||||
|
||||
foreach (var item in listNodesNegative)
|
||||
{
|
||||
if (cancellationToken.HasValue && cancellationToken.Value.IsCancellationRequested) return [];
|
||||
if (nodeCurrent.Parent is null) continue;
|
||||
var nodeDtoCurrent = Nodes.FirstOrDefault(n => n.Id == nodeCurrent.Id);
|
||||
var nodeDtoNegative = Nodes.FirstOrDefault(n => n.Id == item.Id);
|
||||
var nodeDtoParent = Nodes.FirstOrDefault(n => n.Id == nodeCurrent.Parent.Id);
|
||||
var negativeEdge = Edges.FirstOrDefault(e => e.StartNodeId == nodeCurrent.Id && e.EndNodeId == item.Id || e.EndNodeId == nodeCurrent.Id && e.StartNodeId == item.Id);
|
||||
var parentEdge = Edges.FirstOrDefault(e => e.StartNodeId == nodeCurrent.Id && e.EndNodeId == nodeCurrent.Parent.Id || e.EndNodeId == nodeCurrent.Id && e.StartNodeId == nodeCurrent.Parent.Id);
|
||||
|
||||
if (nodeDtoCurrent is null || nodeDtoNegative is null || negativeEdge is null) continue;
|
||||
|
||||
var nearNodeNevgative = MapEditorHelper.GetNearByNode(nodeDtoCurrent, nodeDtoNegative, negativeEdge, 0.01);
|
||||
var nearNodeParent = nodeDtoParent is not null && parentEdge is not null ? MapEditorHelper.GetNearByNode(nodeDtoCurrent, nodeDtoParent, parentEdge, 0.01) :
|
||||
new()
|
||||
{
|
||||
Id = nodeCurrent.Parent.Id,
|
||||
X = nodeCurrent.Parent.X,
|
||||
Y = nodeCurrent.Parent.Y,
|
||||
Name = nodeCurrent.Parent.Name
|
||||
};
|
||||
|
||||
var angle = MapEditorHelper.GetAngle(nodeDtoCurrent, nearNodeNevgative, nearNodeParent);
|
||||
RobotDirection direction = angle < 89 ? nodeCurrent.Direction == RobotDirection.FORWARD ? RobotDirection.BACKWARD : RobotDirection.FORWARD : nodeCurrent.Direction;
|
||||
|
||||
var nodeNegative = new SSEAStarNode
|
||||
{
|
||||
Id = item.Id,
|
||||
X = item.X,
|
||||
Y = item.Y,
|
||||
Name = item.Name,
|
||||
Direction = direction,
|
||||
Parent = nodeCurrent
|
||||
};
|
||||
|
||||
var cost = GetNegativeCost(nodeCurrent, nodeNegative);
|
||||
cost = cost > 0 ? cost : Math.Sqrt(Math.Pow(nodeCurrent.X - nodeNegative.X, 2) + Math.Pow(nodeCurrent.Y - nodeNegative.Y, 2));
|
||||
nodeNegative.Cost = cost + nodeCurrent.Cost + (direction == RobotDirection.BACKWARD ? cost * Math.Sqrt(2) / 2 : 0.0);
|
||||
var distance = Math.Abs(endNode.X - nodeNegative.X) + Math.Abs(endNode.Y - nodeNegative.Y);
|
||||
nodeNegative.Heuristic = distance + (direction == RobotDirection.BACKWARD ? distance : 0.0);
|
||||
possiblePointNegative.Add(nodeNegative);
|
||||
}
|
||||
if (nodeCurrent.NegativeNodes is not null && nodeCurrent.NegativeNodes.Count > 0) possiblePointNegative.AddRange(nodeCurrent.NegativeNodes);
|
||||
return possiblePointNegative;
|
||||
}
|
||||
|
||||
public List<SSEAStarNode> Find(SSEAStarNode startNode, NodeDto endNode, RobotDirection goalDirection, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
try
|
||||
{
|
||||
var activeNodes = new PriorityQueue<SSEAStarNode>((a, b) => a.TotalCost.CompareTo(b.TotalCost));
|
||||
var visitedNodes = new HashSet<SSEAStarNode>();
|
||||
var path = new List<SSEAStarNode>();
|
||||
var shortestPath = new HashSet<SSEAStarNode>();
|
||||
|
||||
activeNodes.Enqueue(startNode);
|
||||
|
||||
while (activeNodes.Count > 0 && (!cancellationToken.HasValue || !cancellationToken.Value.IsCancellationRequested))
|
||||
{
|
||||
var checkNode = activeNodes.Dequeue();
|
||||
if (checkNode.Id == endNode.Id)
|
||||
{
|
||||
if (checkNode.Direction == goalDirection)
|
||||
{
|
||||
var node = checkNode;
|
||||
while (node != null)
|
||||
{
|
||||
if (cancellationToken.HasValue && cancellationToken.Value.IsCancellationRequested) return [];
|
||||
path.Add(node);
|
||||
node = node.Parent;
|
||||
}
|
||||
return path;
|
||||
}
|
||||
else
|
||||
{
|
||||
var node = checkNode;
|
||||
while (node != null)
|
||||
{
|
||||
if (cancellationToken.HasValue && cancellationToken.Value.IsCancellationRequested) return [];
|
||||
shortestPath.Add(node);
|
||||
node = node.Parent;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
visitedNodes.Add(checkNode);
|
||||
|
||||
var listNodeNegative = GetNegativeAStarNode(checkNode, endNode, cancellationToken);
|
||||
foreach (var node in listNodeNegative)
|
||||
{
|
||||
if (visitedNodes.TryGetValue(node, out SSEAStarNode? value) && value is not null)
|
||||
{
|
||||
if (value.TotalCost > node.TotalCost || shortestPath.Any(n => n.Id == node.Id) && value.Parent is not null && value.Parent.Heuristic < checkNode.Heuristic)
|
||||
{
|
||||
visitedNodes.Remove(value);
|
||||
activeNodes.Enqueue(node);
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
var activeNode = activeNodes.Items.FirstOrDefault(n => n.Id == node.Id && n.Direction == node.Direction);
|
||||
if (activeNode is not null && activeNode.TotalCost > node.TotalCost)
|
||||
{
|
||||
activeNodes.Items.Remove(activeNode);
|
||||
activeNodes.Enqueue(node);
|
||||
}
|
||||
else
|
||||
{
|
||||
activeNodes.Enqueue(node);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return [];
|
||||
}
|
||||
catch
|
||||
{
|
||||
return [];
|
||||
}
|
||||
}
|
||||
|
||||
public List<SSEAStarNode> Find(SSEAStarNode startNode, NodeDto endNode, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
try
|
||||
{
|
||||
var activeNodes = new PriorityQueue<SSEAStarNode>((a, b) => a.TotalCost.CompareTo(b.TotalCost));
|
||||
var visitedNodes = new HashSet<SSEAStarNode>();
|
||||
var path = new List<SSEAStarNode>();
|
||||
var shortestPath = new HashSet<SSEAStarNode>();
|
||||
|
||||
activeNodes.Enqueue(startNode);
|
||||
|
||||
while (activeNodes.Count > 0 && (!cancellationToken.HasValue || !cancellationToken.Value.IsCancellationRequested))
|
||||
{
|
||||
var checkNode = activeNodes.Dequeue();
|
||||
if (checkNode.Id == endNode.Id)
|
||||
{
|
||||
if (checkNode.Parent is not null)
|
||||
{
|
||||
var nodeParentDto = Nodes.FirstOrDefault(n => n.Id == checkNode.Parent.Id);
|
||||
var edge = Edges.FirstOrDefault(e => e.StartNodeId == checkNode.Id && e.EndNodeId == checkNode.Parent.Id || e.EndNodeId == checkNode.Id && e.StartNodeId == checkNode.Parent.Id);
|
||||
if (edge is not null && nodeParentDto is not null)
|
||||
{
|
||||
var nearParent = MapEditorHelper.GetNearByNode(endNode, nodeParentDto, edge, 0.01);
|
||||
var nearGoalNode = new NodeDto()
|
||||
{
|
||||
X = endNode.X + Math.Cos(endNode.Theta * Math.PI / 180),
|
||||
Y = endNode.Y + Math.Sin(endNode.Theta * Math.PI / 180),
|
||||
};
|
||||
|
||||
var angle = MapEditorHelper.GetAngle(endNode, nearParent, nearGoalNode);
|
||||
RobotDirection goalDirection = angle < 89 ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
|
||||
if (checkNode.Direction == goalDirection)
|
||||
{
|
||||
var returnNode = checkNode;
|
||||
while (returnNode != null)
|
||||
{
|
||||
if (cancellationToken.HasValue && cancellationToken.Value.IsCancellationRequested) return [];
|
||||
path.Add(returnNode);
|
||||
returnNode = returnNode.Parent;
|
||||
}
|
||||
return path;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
var node = checkNode;
|
||||
while (node != null)
|
||||
{
|
||||
if (cancellationToken.HasValue && cancellationToken.Value.IsCancellationRequested) return [];
|
||||
shortestPath.Add(node);
|
||||
node = node.Parent;
|
||||
}
|
||||
}
|
||||
|
||||
visitedNodes.Add(checkNode);
|
||||
|
||||
var listNodeNegative = GetNegativeAStarNode(checkNode, endNode, cancellationToken);
|
||||
foreach (var node in listNodeNegative)
|
||||
{
|
||||
if (visitedNodes.TryGetValue(node, out SSEAStarNode? value) && value is not null)
|
||||
{
|
||||
if (value.TotalCost > node.TotalCost || shortestPath.Any(n => n.Id == node.Id) && value.Parent is not null && value.Parent.Heuristic < checkNode.Heuristic)
|
||||
{
|
||||
visitedNodes.Remove(value);
|
||||
activeNodes.Enqueue(node);
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
var activeNode = activeNodes.Items.FirstOrDefault(n => n.Id == node.Id && n.Direction == node.Direction);
|
||||
if (activeNode is not null && activeNode.TotalCost > node.TotalCost)
|
||||
{
|
||||
activeNodes.Items.Remove(activeNode);
|
||||
activeNodes.Enqueue(node);
|
||||
}
|
||||
else
|
||||
{
|
||||
activeNodes.Enqueue(node);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return [];
|
||||
}
|
||||
catch
|
||||
{
|
||||
return [];
|
||||
}
|
||||
}
|
||||
|
||||
private SSEAStarNode GetClosesNode(NodeDto closesNode, NodeDto goal, double theta, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
SSEAStarNode closesAStarNode = new()
|
||||
{
|
||||
Id = closesNode.Id,
|
||||
X = closesNode.X,
|
||||
Y = closesNode.Y,
|
||||
Name = closesNode.Name,
|
||||
};
|
||||
foreach (var negativeNode in GetNegativeNode(closesAStarNode.Id, cancellationToken))
|
||||
{
|
||||
SSEAStarNode closesAStarNodeParent = new()
|
||||
{
|
||||
Id = closesNode.Id,
|
||||
X = closesNode.X,
|
||||
Y = closesNode.Y,
|
||||
Name = closesNode.Name,
|
||||
};
|
||||
var RobotNearNode = new NodeDto()
|
||||
{
|
||||
X = closesAStarNode.X + Math.Cos(theta * Math.PI / 180),
|
||||
Y = closesAStarNode.Y + Math.Sin(theta * Math.PI / 180),
|
||||
};
|
||||
|
||||
var angle = MapEditorHelper.GetAngle(closesNode, negativeNode, RobotNearNode);
|
||||
RobotDirection direction = angle < 91 ? RobotDirection.FORWARD : RobotDirection.BACKWARD;
|
||||
|
||||
var cost = GetNegativeCost(closesAStarNode, new() { Id = negativeNode.Id, X = negativeNode.X, Y = negativeNode.Y });
|
||||
cost = cost > 0 ? cost : Math.Sqrt(Math.Pow(closesAStarNode.X - negativeNode.X, 2) + Math.Pow(closesAStarNode.Y - negativeNode.Y, 2));
|
||||
cost += direction == RobotDirection.BACKWARD ? cost * Math.Sqrt(2) / 2 : 0.0;
|
||||
closesAStarNodeParent.Direction = direction;
|
||||
closesAStarNode.NegativeNodes.Add(new()
|
||||
{
|
||||
Id = negativeNode.Id,
|
||||
X = negativeNode.X,
|
||||
Y = negativeNode.Y,
|
||||
Name = negativeNode.Name,
|
||||
Direction = direction,
|
||||
Cost = cost,
|
||||
Heuristic = Math.Abs(goal.X - negativeNode.X) + Math.Abs(goal.Y - negativeNode.Y),
|
||||
Parent = closesAStarNodeParent,
|
||||
});
|
||||
}
|
||||
return closesAStarNode;
|
||||
}
|
||||
|
||||
private static SSEAStarNode[] GetClosesEdge(EdgeDto closesEdge, NodeDto goal, NodeDto startNodeForward, NodeDto startNodeBackward, SSEAStarNode robotNode, double theta)
|
||||
{
|
||||
List<SSEAStarNode> negativeNodes = [];
|
||||
if (closesEdge.DirectionAllowed == DirectionAllowed.Both || closesEdge.DirectionAllowed == DirectionAllowed.Backward)
|
||||
{
|
||||
SSEAStarNode closesAStarNodeParent = new()
|
||||
{
|
||||
Id = robotNode.Id,
|
||||
X = robotNode.X,
|
||||
Y = robotNode.Y,
|
||||
Name = robotNode.Name,
|
||||
};
|
||||
var RobotNearNode = new NodeDto()
|
||||
{
|
||||
X = robotNode.X + Math.Cos(theta * Math.PI / 180),
|
||||
Y = robotNode.Y + Math.Sin(theta * Math.PI / 180),
|
||||
};
|
||||
var angle = MapEditorHelper.GetAngle(new() { X = robotNode.X, Y = robotNode.Y, Theta = theta }, startNodeForward, RobotNearNode);
|
||||
RobotDirection direction = angle < 91 ? RobotDirection.FORWARD : RobotDirection.BACKWARD;
|
||||
|
||||
double cost = Math.Sqrt(Math.Pow(robotNode.X - startNodeBackward.X, 2) + Math.Pow(robotNode.Y - startNodeBackward.Y, 2));
|
||||
cost += direction == RobotDirection.BACKWARD ? cost * Math.Sqrt(2) / 2 : 0.0;
|
||||
closesAStarNodeParent.Direction = direction;
|
||||
negativeNodes.Add(new()
|
||||
{
|
||||
Id = startNodeForward.Id,
|
||||
X = startNodeForward.X,
|
||||
Y = startNodeForward.Y,
|
||||
Name = startNodeForward.Name,
|
||||
Direction = direction,
|
||||
Cost = cost,
|
||||
Heuristic = Math.Abs(goal.X - startNodeForward.X) + Math.Abs(goal.Y - startNodeForward.Y),
|
||||
Parent = closesAStarNodeParent,
|
||||
});
|
||||
}
|
||||
if (closesEdge.DirectionAllowed == DirectionAllowed.Both || closesEdge.DirectionAllowed == DirectionAllowed.Forward)
|
||||
{
|
||||
SSEAStarNode closesAStarNodeParent = new()
|
||||
{
|
||||
Id = robotNode.Id,
|
||||
X = robotNode.X,
|
||||
Y = robotNode.Y,
|
||||
Name = robotNode.Name,
|
||||
};
|
||||
var RobotNearNode = new NodeDto()
|
||||
{
|
||||
X = robotNode.X + Math.Cos(theta * Math.PI / 180),
|
||||
Y = robotNode.Y + Math.Sin(theta * Math.PI / 180),
|
||||
};
|
||||
var angle = MapEditorHelper.GetAngle(new() { X = robotNode.X, Y = robotNode.Y, Theta = theta }, startNodeBackward, RobotNearNode);
|
||||
RobotDirection direction = angle < 91 ? RobotDirection.FORWARD : RobotDirection.BACKWARD;
|
||||
|
||||
double cost = Math.Sqrt(Math.Pow(robotNode.X - startNodeBackward.X, 2) + Math.Pow(robotNode.Y - startNodeBackward.Y, 2));
|
||||
cost += direction == RobotDirection.BACKWARD ? cost * Math.Sqrt(2) / 2 : 0.0;
|
||||
closesAStarNodeParent.Direction = direction;
|
||||
negativeNodes.Add(new()
|
||||
{
|
||||
Id = startNodeBackward.Id,
|
||||
X = startNodeBackward.X,
|
||||
Y = startNodeBackward.Y,
|
||||
Name = startNodeBackward.Name,
|
||||
Direction = direction,
|
||||
Cost = cost,
|
||||
Heuristic = Math.Abs(goal.X - startNodeBackward.X) + Math.Abs(goal.Y - startNodeBackward.Y),
|
||||
Parent = closesAStarNodeParent,
|
||||
});
|
||||
}
|
||||
return [.. negativeNodes];
|
||||
}
|
||||
|
||||
public MessageResult<List<NodeDto>> PlanningWithFinalDirection(double x, double y, double theta, NodeDto goal, RobotDirection goalDirection, double maxDistanceToEdge, double maxDistanceToNode, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var Path = new List<NodeDto>();
|
||||
|
||||
SSEAStarNode 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 = GetClosesNode(closesNode, goal, theta);
|
||||
}
|
||||
else
|
||||
{
|
||||
var closesEdge = GetClosesEdge(x, 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 };
|
||||
}
|
||||
RobotNode.NegativeNodes.AddRange(GetClosesEdge(closesEdge, goal, startNodeForward, startNodeBackward, RobotNode, theta));
|
||||
}
|
||||
|
||||
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, goalDirection, 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.Id == path.First().Id)
|
||||
{
|
||||
Path.Add(new()
|
||||
{
|
||||
Id = node.Id,
|
||||
Name = node.Name,
|
||||
X = node.X,
|
||||
Y = node.Y,
|
||||
Direction = MapCompute.GetNodeDirection(node.Direction),
|
||||
});
|
||||
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");
|
||||
nodedb.Direction = MapCompute.GetNodeDirection(node.Direction);
|
||||
Path.Add(nodedb);
|
||||
}
|
||||
return new(true) { Data = Path };
|
||||
}
|
||||
|
||||
public MessageResult<List<NodeDto>> PlanningWithGoalAngle(double x, double y, double theta, NodeDto goal, double maxDistanceToEdge, double maxDistanceToNode, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var Path = new List<NodeDto>();
|
||||
|
||||
SSEAStarNode 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 = GetClosesNode(closesNode, goal, theta);
|
||||
}
|
||||
else
|
||||
{
|
||||
var closesEdge = GetClosesEdge(x, 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 };
|
||||
}
|
||||
RobotNode.NegativeNodes.AddRange(GetClosesEdge(closesEdge, goal, startNodeForward, startNodeBackward, RobotNode, theta));
|
||||
}
|
||||
|
||||
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.Id == path.First().Id)
|
||||
{
|
||||
Path.Add(new()
|
||||
{
|
||||
Id = node.Id,
|
||||
Name = node.Name,
|
||||
X = node.X,
|
||||
Y = node.Y,
|
||||
Direction = MapCompute.GetNodeDirection(node.Direction),
|
||||
});
|
||||
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");
|
||||
nodedb.Direction = MapCompute.GetNodeDirection(node.Direction);
|
||||
Path.Add(nodedb);
|
||||
}
|
||||
return new(true) { Data = Path };
|
||||
}
|
||||
|
||||
public MessageResult<NodeDto[]> PlanningWithFinalDirection(NodeDto startNode, double theta, NodeDto goal, RobotDirection goalDirection, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var Path = new List<NodeDto>();
|
||||
SSEAStarNode RobotNode = GetClosesNode(startNode, goal, theta);
|
||||
var path = Find(RobotNode, goal, goalDirection, 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");
|
||||
nodedb.Direction = MapCompute.GetNodeDirection(node.Direction);
|
||||
Path.Add(nodedb);
|
||||
}
|
||||
return new(true) { Data = [.. Path] };
|
||||
}
|
||||
|
||||
public MessageResult<NodeDto[]> PlanningWithGoalAngle(NodeDto startNode, double theta, NodeDto goal, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var Path = new List<NodeDto>();
|
||||
SSEAStarNode RobotNode = GetClosesNode(startNode, goal, theta);
|
||||
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ừ [{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");
|
||||
nodedb.Direction = MapCompute.GetNodeDirection(node.Direction);
|
||||
Path.Add(nodedb);
|
||||
}
|
||||
return new(true) { Data = [.. Path] };
|
||||
}
|
||||
}
|
||||
20
RobotNet.RobotManager/Services/Planner/IPathPlanner.cs
Normal file
20
RobotNet.RobotManager/Services/Planner/IPathPlanner.cs
Normal file
@@ -0,0 +1,20 @@
|
||||
using RobotNet.MapShares.Dtos;
|
||||
using RobotNet.RobotShares.Enums;
|
||||
using RobotNet.Shares;
|
||||
|
||||
namespace RobotNet.RobotManager.Services.Planner;
|
||||
|
||||
public interface IPathPlanner
|
||||
{
|
||||
MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanning(double x, double y, double theta, Guid goalId, CancellationToken? cancellationToken = null);
|
||||
MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithStartDirection(double x, double y, double theta, Guid goalId, RobotDirection startDiretion = RobotDirection.NONE, CancellationToken? cancellationToken = null);
|
||||
MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithFinalDirection(double x, double y, double theta, Guid goalId, RobotDirection goalDirection = RobotDirection.NONE, CancellationToken? cancellationToken = null);
|
||||
MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithAngle(double x, double y, double theta, Guid goalId, CancellationToken? cancellationToken = null);
|
||||
|
||||
MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanning(Guid startNodeId, double theta, Guid goalId, CancellationToken? cancellationToken = null);
|
||||
MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithStartDirection(Guid startNodeId, double theta, Guid goalId, RobotDirection startDiretion = RobotDirection.NONE, CancellationToken? cancellationToken = null);
|
||||
MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithFinalDirection(Guid startNodeId, double theta, Guid goalId, RobotDirection goalDirection = RobotDirection.NONE, CancellationToken? cancellationToken = null);
|
||||
MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithAngle(Guid startNodeId, double theta, Guid goalId, CancellationToken? cancellationToken = null);
|
||||
void SetData(NodeDto[] nodes, EdgeDto[] edges);
|
||||
void SetOptions(PathPlanningOptions options);
|
||||
}
|
||||
@@ -0,0 +1,8 @@
|
||||
using RobotNet.RobotShares.Enums;
|
||||
|
||||
namespace RobotNet.RobotManager.Services.Planner;
|
||||
|
||||
public interface IPathPlannerManager
|
||||
{
|
||||
IPathPlanner GetPathPlanningService(NavigationType type);
|
||||
}
|
||||
@@ -0,0 +1,265 @@
|
||||
using RobotNet.MapShares;
|
||||
using RobotNet.MapShares.Dtos;
|
||||
using RobotNet.MapShares.Enums;
|
||||
using RobotNet.RobotManager.Services.Planner.AStar;
|
||||
using RobotNet.RobotManager.Services.Planner.Space;
|
||||
using RobotNet.RobotShares.Enums;
|
||||
using RobotNet.Shares;
|
||||
|
||||
namespace RobotNet.RobotManager.Services.Planner.OmniDrive;
|
||||
|
||||
public class OmniDrivePathPlanner : IPathPlanner
|
||||
{
|
||||
private List<NodeDto> Nodes = [];
|
||||
private List<EdgeDto> Edges = [];
|
||||
private const double ReverseDirectionAngleDegree = 89;
|
||||
private const double Ratio = 0.1;
|
||||
|
||||
private readonly PathPlanningOptions Options = new()
|
||||
{
|
||||
LimitDistanceToEdge = 1,
|
||||
LimitDistanceToNode = 0.3,
|
||||
ResolutionSplit = 0.1,
|
||||
};
|
||||
|
||||
public void SetData(NodeDto[] nodes, EdgeDto[] edges)
|
||||
{
|
||||
Nodes = [.. nodes];
|
||||
Edges = [.. edges];
|
||||
}
|
||||
|
||||
public void SetOptions(PathPlanningOptions options)
|
||||
{
|
||||
Options.LimitDistanceToNode = options.LimitDistanceToNode;
|
||||
Options.LimitDistanceToEdge = options.LimitDistanceToEdge;
|
||||
Options.ResolutionSplit = options.ResolutionSplit;
|
||||
}
|
||||
|
||||
public static RobotDirection[] GetRobotDirectionInPath(RobotDirection currentDirection, List<NodeDto> nodeplanning, List<EdgeDto> edgePlanning)
|
||||
{
|
||||
RobotDirection[] RobotDirectionInNode = new RobotDirection[nodeplanning.Count];
|
||||
if (nodeplanning.Count > 0) RobotDirectionInNode[0] = currentDirection;
|
||||
if (nodeplanning.Count > 2)
|
||||
{
|
||||
for (int i = 1; i < nodeplanning.Count - 1; i++)
|
||||
{
|
||||
NodeDto startNode = MapEditorHelper.GetNearByNode(nodeplanning[i], nodeplanning[i - 1], edgePlanning[i - 1], Ratio);
|
||||
NodeDto endNode = MapEditorHelper.GetNearByNode(nodeplanning[i], nodeplanning[i + 1], edgePlanning[i], Ratio); ;
|
||||
var angle = MapEditorHelper.GetAngle(nodeplanning[i], startNode, endNode);
|
||||
if (angle < ReverseDirectionAngleDegree) RobotDirectionInNode[i] = RobotDirectionInNode[i - 1] == RobotDirection.FORWARD ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
|
||||
else RobotDirectionInNode[i] = RobotDirectionInNode[i - 1];
|
||||
}
|
||||
}
|
||||
if (nodeplanning.Count > 1) RobotDirectionInNode[^1] = RobotDirectionInNode[^2];
|
||||
return RobotDirectionInNode;
|
||||
}
|
||||
|
||||
public static RobotDirection GetRobotDirection(NodeDto currentNode, NodeDto nearNode, EdgeDto edge, double robotInNodeAngle, bool isReverse)
|
||||
{
|
||||
NodeDto NearNode = MapEditorHelper.GetNearByNode(currentNode, nearNode, edge, Ratio);
|
||||
|
||||
var RobotNearNode = new NodeDto()
|
||||
{
|
||||
X = currentNode.X + Math.Cos(robotInNodeAngle * Math.PI / 180),
|
||||
Y = currentNode.Y + Math.Sin(robotInNodeAngle * Math.PI / 180),
|
||||
};
|
||||
var angle = MapEditorHelper.GetAngle(currentNode, NearNode, RobotNearNode);
|
||||
if (angle > ReverseDirectionAngleDegree) return isReverse ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
|
||||
else return isReverse ? RobotDirection.FORWARD : RobotDirection.BACKWARD;
|
||||
}
|
||||
|
||||
private EdgeDto[] GetEdgesPlanning(NodeDto[] nodes)
|
||||
{
|
||||
var EdgesPlanning = new List<EdgeDto>();
|
||||
for (int i = 0; i < nodes.Length - 1; i++)
|
||||
{
|
||||
var edge = Edges.FirstOrDefault(e => e.StartNodeId == nodes[i].Id && e.EndNodeId == nodes[i + 1].Id ||
|
||||
e.EndNodeId == nodes[i].Id && e.StartNodeId == nodes[i + 1].Id);
|
||||
if (edge is null)
|
||||
{
|
||||
if (i != 0) return [];
|
||||
EdgesPlanning.Add(new EdgeDto()
|
||||
{
|
||||
Id = Guid.NewGuid(),
|
||||
StartNodeId = nodes[i].Id,
|
||||
EndNodeId = nodes[i + 1].Id,
|
||||
DirectionAllowed = DirectionAllowed.Both,
|
||||
TrajectoryDegree = TrajectoryDegree.One,
|
||||
});
|
||||
continue;
|
||||
}
|
||||
bool isReverse = nodes[i].Id != edge.StartNodeId && edge.TrajectoryDegree == TrajectoryDegree.Three;
|
||||
EdgesPlanning.Add(new()
|
||||
{
|
||||
Id = edge.Id,
|
||||
StartNodeId = nodes[i].Id,
|
||||
EndNodeId = nodes[i + 1].Id,
|
||||
DirectionAllowed = edge.DirectionAllowed,
|
||||
TrajectoryDegree = edge.TrajectoryDegree,
|
||||
ControlPoint1X = isReverse ? edge.ControlPoint2X : edge.ControlPoint1X,
|
||||
ControlPoint1Y = isReverse ? edge.ControlPoint2Y : edge.ControlPoint1Y,
|
||||
ControlPoint2X = isReverse ? edge.ControlPoint1X : edge.ControlPoint2X,
|
||||
ControlPoint2Y = isReverse ? edge.ControlPoint1Y : edge.ControlPoint2Y
|
||||
});
|
||||
}
|
||||
return [.. EdgesPlanning];
|
||||
}
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanning(double x, double y, double theta, Guid goalId, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
|
||||
if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
|
||||
|
||||
var AStarPathPlanner = new AStarPathPlanner(Nodes, Edges);
|
||||
var Path = AStarPathPlanner.Planning(x,
|
||||
y,
|
||||
new NodeDto() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
|
||||
Options.LimitDistanceToEdge,
|
||||
Options.LimitDistanceToNode,
|
||||
cancellationToken);
|
||||
if (!Path.IsSuccess) return new(false, Path.Message);
|
||||
if (Path.Data is null || Path.Data.Count < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{x}, {y}, {theta}]");
|
||||
if (Path.Data.Count == 1) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
|
||||
|
||||
var edgeplannings = GetEdgesPlanning([.. Path.Data]);
|
||||
|
||||
RobotDirection CurrenDirection = GetRobotDirection(Path.Data[0], Path.Data[1], edgeplannings[0], theta, true);
|
||||
var FinalDirection = GetRobotDirectionInPath(CurrenDirection, Path.Data, [.. edgeplannings]);
|
||||
foreach (var item in Path.Data)
|
||||
{
|
||||
item.Direction = MapCompute.GetNodeDirection(FinalDirection[Path.Data.IndexOf(item)]);
|
||||
}
|
||||
|
||||
return new(true)
|
||||
{
|
||||
Data = ([.. Path.Data], [.. edgeplannings])
|
||||
};
|
||||
}
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithAngle(double x, double y, double theta, Guid goalId, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
|
||||
if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
|
||||
|
||||
var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
|
||||
if (!basicPath.IsSuccess) return basicPath;
|
||||
if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
|
||||
|
||||
RobotDirection goalDirection = GetRobotDirection(basicPath.Data.Nodes[^1], basicPath.Data.Nodes[^2], basicPath.Data.Edges[^1], goal.Theta, false);
|
||||
if (MapCompute.GetNodeDirection(goalDirection) == basicPath.Data.Nodes[^1].Direction) return basicPath;
|
||||
foreach (var node in basicPath.Data.Nodes)
|
||||
{
|
||||
node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
|
||||
}
|
||||
|
||||
return basicPath;
|
||||
}
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithStartDirection(double x, double y, double theta, Guid goalId, RobotDirection startDiretion = RobotDirection.NONE, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
|
||||
if (!basicPath.IsSuccess) return basicPath;
|
||||
if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
|
||||
|
||||
if (MapCompute.GetNodeDirection(startDiretion) == basicPath.Data.Nodes[0].Direction || startDiretion == RobotDirection.NONE) return basicPath;
|
||||
foreach (var node in basicPath.Data.Nodes)
|
||||
{
|
||||
node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
|
||||
}
|
||||
|
||||
return basicPath;
|
||||
}
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithFinalDirection(double x, double y, double theta, Guid goalId, RobotDirection goalDirection = RobotDirection.NONE, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
|
||||
if (!basicPath.IsSuccess) return basicPath;
|
||||
if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
|
||||
|
||||
if (MapCompute.GetNodeDirection(goalDirection) == basicPath.Data.Nodes[^1].Direction || goalDirection == RobotDirection.NONE) return basicPath;
|
||||
foreach (var node in basicPath.Data.Nodes)
|
||||
{
|
||||
node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
|
||||
}
|
||||
|
||||
return basicPath;
|
||||
}
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanning(Guid startNodeId, double theta, Guid goalId, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
|
||||
if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
|
||||
|
||||
var startNode = Nodes.FirstOrDefault(n => n.Id == startNodeId);
|
||||
if (startNode is null) return new(false, $"Điểm bắt đầu {startNodeId} không tồn tại trong map");
|
||||
|
||||
var AStarPathPlanner = new AStarPathPlanner(Nodes, Edges);
|
||||
var Path = AStarPathPlanner.Planning(startNode, goal, cancellationToken);
|
||||
if (!Path.IsSuccess) return new(false, Path.Message);
|
||||
if (Path.Data is null || Path.Data.Length < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{startNode.Name} - {startNode.Id}]");
|
||||
if (Path.Data.Length == 1) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
|
||||
|
||||
var edgeplannings = GetEdgesPlanning([.. Path.Data]);
|
||||
|
||||
RobotDirection CurrenDirection = GetRobotDirection(Path.Data[0], Path.Data[1], edgeplannings[0], theta, true);
|
||||
var FinalDirection = GetRobotDirectionInPath(CurrenDirection, [.. Path.Data], [.. edgeplannings]);
|
||||
for (int i = 0; i < Path.Data.Length; i++)
|
||||
{
|
||||
Path.Data[i].Direction = MapCompute.GetNodeDirection(FinalDirection[i]);
|
||||
}
|
||||
|
||||
return new(true)
|
||||
{
|
||||
Data = ([.. Path.Data], [.. edgeplannings])
|
||||
};
|
||||
}
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithAngle(Guid startNodeId, double theta, Guid goalId, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
|
||||
if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
|
||||
|
||||
var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
|
||||
if (!basicPath.IsSuccess) return basicPath;
|
||||
if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
|
||||
|
||||
RobotDirection goalDirection = GetRobotDirection(basicPath.Data.Nodes[^1], basicPath.Data.Nodes[^2], basicPath.Data.Edges[^1], goal.Theta, false);
|
||||
if (MapCompute.GetNodeDirection(goalDirection) == basicPath.Data.Nodes[^1].Direction) return basicPath;
|
||||
foreach (var node in basicPath.Data.Nodes)
|
||||
{
|
||||
node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
|
||||
}
|
||||
|
||||
return basicPath;
|
||||
}
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithStartDirection(Guid startNodeId, double theta, Guid goalId, RobotDirection startDiretion = RobotDirection.NONE, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
|
||||
if (!basicPath.IsSuccess) return basicPath;
|
||||
if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
|
||||
|
||||
if (MapCompute.GetNodeDirection(startDiretion) == basicPath.Data.Nodes[0].Direction || startDiretion == RobotDirection.NONE) return basicPath;
|
||||
foreach (var node in basicPath.Data.Nodes)
|
||||
{
|
||||
node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
|
||||
}
|
||||
|
||||
return basicPath;
|
||||
}
|
||||
|
||||
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithFinalDirection(Guid startNodeId, double theta, Guid goalId, RobotDirection goalDirection = RobotDirection.NONE, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
|
||||
if (!basicPath.IsSuccess) return basicPath;
|
||||
if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
|
||||
|
||||
if (MapCompute.GetNodeDirection(goalDirection) == basicPath.Data.Nodes[^1].Direction || goalDirection == RobotDirection.NONE) return basicPath;
|
||||
foreach (var node in basicPath.Data.Nodes)
|
||||
{
|
||||
node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
|
||||
}
|
||||
|
||||
return basicPath;
|
||||
}
|
||||
}
|
||||
19
RobotNet.RobotManager/Services/Planner/PathPlannerManager.cs
Normal file
19
RobotNet.RobotManager/Services/Planner/PathPlannerManager.cs
Normal file
@@ -0,0 +1,19 @@
|
||||
using RobotNet.RobotManager.Services.Planner.Differential;
|
||||
using RobotNet.RobotManager.Services.Planner.ForkliftV2;
|
||||
using RobotNet.RobotManager.Services.Planner.OmniDrive;
|
||||
using RobotNet.RobotShares.Enums;
|
||||
|
||||
namespace RobotNet.RobotManager.Services.Planner;
|
||||
|
||||
public class PathPlannerManager : IPathPlannerManager
|
||||
{
|
||||
public IPathPlanner GetPathPlanningService(NavigationType type)
|
||||
{
|
||||
return type switch
|
||||
{
|
||||
NavigationType.Forklift => new ForkLiftPathPlannerV2(),
|
||||
NavigationType.OmniDrive => new OmniDrivePathPlanner(),
|
||||
_ => new DifferentialPathPlanner(),
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,8 @@
|
||||
namespace RobotNet.RobotManager.Services.Planner;
|
||||
|
||||
public class PathPlanningOptions
|
||||
{
|
||||
public double LimitDistanceToEdge { get; set; }
|
||||
public double LimitDistanceToNode { get; set; }
|
||||
public double ResolutionSplit { get; set; }
|
||||
}
|
||||
23
RobotNet.RobotManager/Services/Planner/PriorityQueue.cs
Normal file
23
RobotNet.RobotManager/Services/Planner/PriorityQueue.cs
Normal file
@@ -0,0 +1,23 @@
|
||||
namespace RobotNet.RobotManager.Services.Planner;
|
||||
|
||||
public class PriorityQueue<T>(Comparison<T> comparison)
|
||||
{
|
||||
public List<T> Items => items;
|
||||
private readonly List<T> items = [];
|
||||
|
||||
public void Enqueue(T item)
|
||||
{
|
||||
items.Add(item);
|
||||
items.Sort(comparison);
|
||||
}
|
||||
|
||||
public T Dequeue()
|
||||
{
|
||||
if (items.Count == 0) throw new InvalidOperationException("Queue is empty");
|
||||
var item = items[0];
|
||||
items.RemoveAt(0);
|
||||
return item;
|
||||
}
|
||||
|
||||
public int Count => items.Count;
|
||||
}
|
||||
61
RobotNet.RobotManager/Services/Planner/Space/KDTree.cs
Normal file
61
RobotNet.RobotManager/Services/Planner/Space/KDTree.cs
Normal file
@@ -0,0 +1,61 @@
|
||||
using RobotNet.MapShares.Dtos;
|
||||
|
||||
namespace RobotNet.RobotManager.Services.Planner.Space;
|
||||
|
||||
public class KDTree(List<NodeDto> nodes)
|
||||
{
|
||||
private readonly KDTreeNode? Root = BuildTree(nodes, 0);
|
||||
|
||||
private static KDTreeNode? BuildTree(List<NodeDto> nodes, int depth)
|
||||
{
|
||||
if (nodes.Count == 0) return null;
|
||||
int axis = depth % 2;
|
||||
nodes.Sort((a, b) => axis == 0 ? a.X.CompareTo(b.X) : a.Y.CompareTo(b.Y));
|
||||
int median = nodes.Count / 2;
|
||||
|
||||
return new KDTreeNode
|
||||
{
|
||||
Node = nodes[median],
|
||||
Axis = axis,
|
||||
Left = BuildTree(nodes.GetRange(0, median), depth + 1),
|
||||
Right = BuildTree(nodes.GetRange(median + 1, nodes.Count - median - 1), depth + 1)
|
||||
};
|
||||
}
|
||||
|
||||
public NodeDto? FindNearest(double x, double y, double limitDistance)
|
||||
{
|
||||
if (Root is null) return null;
|
||||
var (node, dist) = Nearest(Root, x, y, null, double.MaxValue);
|
||||
return dist <= limitDistance ? node : null;
|
||||
}
|
||||
|
||||
private static (NodeDto?, double) Nearest(KDTreeNode? node, double x, double y, NodeDto? best, double bestDist)
|
||||
{
|
||||
if (node == null) return (best, bestDist);
|
||||
|
||||
double d = Math.Sqrt(Math.Pow(node.Node.X - x, 2) + Math.Pow(node.Node.Y - y, 2));
|
||||
if (d < bestDist)
|
||||
{
|
||||
best = node.Node;
|
||||
bestDist = d;
|
||||
}
|
||||
|
||||
double delta = node.Axis == 0 ? x - node.Node.X : y - node.Node.Y;
|
||||
KDTreeNode? nearSide = delta < 0 ? node.Left : node.Right;
|
||||
KDTreeNode? farSide = delta < 0 ? node.Right : node.Left;
|
||||
|
||||
(best, bestDist) = Nearest(nearSide, x, y, best, bestDist);
|
||||
if (Math.Abs(delta) < bestDist)
|
||||
(best, bestDist) = Nearest(farSide, x, y, best, bestDist);
|
||||
|
||||
return (best, bestDist);
|
||||
}
|
||||
}
|
||||
|
||||
public class KDTreeNode
|
||||
{
|
||||
public NodeDto Node { get; set; } = new();
|
||||
public KDTreeNode? Left { get; set; }
|
||||
public KDTreeNode? Right { get; set; }
|
||||
public int Axis { get; set; } // 0 for X, 1 for Y
|
||||
}
|
||||
86
RobotNet.RobotManager/Services/Planner/Space/MapCompute.cs
Normal file
86
RobotNet.RobotManager/Services/Planner/Space/MapCompute.cs
Normal file
@@ -0,0 +1,86 @@
|
||||
using RobotNet.MapShares;
|
||||
using RobotNet.MapShares.Dtos;
|
||||
using RobotNet.MapShares.Enums;
|
||||
using RobotNet.RobotShares.Enums;
|
||||
using RobotNet.Shares;
|
||||
|
||||
namespace RobotNet.RobotManager.Services.Planner.Space;
|
||||
|
||||
public class MapCompute
|
||||
{
|
||||
public static double DistanceToCurveEdge(double x, double y, EdgeDto edge, NodeDto startNode, NodeDto endNode)
|
||||
{
|
||||
double dMin = Math.Sqrt(Math.Pow(x - startNode.X, 2) + Math.Pow(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)
|
||||
{
|
||||
(double curveX, double curveY) = MapEditorHelper.Curve(t, new()
|
||||
{
|
||||
TrajectoryDegree = edge.TrajectoryDegree,
|
||||
ControlPoint1X = edge.ControlPoint1X,
|
||||
ControlPoint1Y = edge.ControlPoint1Y,
|
||||
ControlPoint2X = edge.ControlPoint2X,
|
||||
ControlPoint2Y= edge.ControlPoint2Y,
|
||||
X1 = startNode.X,
|
||||
Y1 = startNode.Y,
|
||||
X2 = endNode.X,
|
||||
Y2 = endNode.Y,
|
||||
});
|
||||
double d = Math.Sqrt(Math.Pow(x - curveX, 2) + Math.Pow(y - curveY, 2));
|
||||
if (d < dMin) dMin = d;
|
||||
}
|
||||
|
||||
return dMin;
|
||||
}
|
||||
|
||||
public static MessageResult<double> DistanceToEdge(double x, double y, 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, ((x - startNode.X) * (endNode.X - startNode.X) + (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(x - nearestX, 2) + Math.Pow(y - nearestY, 2)) };
|
||||
}
|
||||
else
|
||||
{
|
||||
return new(true) { Data = DistanceToCurveEdge(x, y, edge, startNode, endNode) };
|
||||
}
|
||||
}
|
||||
|
||||
public static Direction GetNodeDirection(RobotDirection robotDirection) =>
|
||||
robotDirection switch
|
||||
{
|
||||
RobotDirection.FORWARD => Direction.FORWARD,
|
||||
RobotDirection.BACKWARD => Direction.BACKWARD,
|
||||
_ => Direction.NONE
|
||||
};
|
||||
|
||||
public static RobotDirection GetRobotDirection(Direction nodeDirection) =>
|
||||
nodeDirection switch
|
||||
{
|
||||
Direction.FORWARD => RobotDirection.FORWARD,
|
||||
Direction.BACKWARD => RobotDirection.BACKWARD,
|
||||
_ => RobotDirection.NONE
|
||||
};
|
||||
}
|
||||
206
RobotNet.RobotManager/Services/Planner/Space/RTree.cs
Normal file
206
RobotNet.RobotManager/Services/Planner/Space/RTree.cs
Normal file
@@ -0,0 +1,206 @@
|
||||
using RobotNet.MapShares;
|
||||
using RobotNet.MapShares.Dtos;
|
||||
using RobotNet.MapShares.Enums;
|
||||
|
||||
namespace RobotNet.RobotManager.Services.Planner.Space;
|
||||
|
||||
public class Rectangle(double minX, double minY, double maxX, double maxY)
|
||||
{
|
||||
public double MinX { get; set; } = minX;
|
||||
public double MinY { get; set; } = minY;
|
||||
public double MaxX { get; set; } = maxX;
|
||||
public double MaxY { get; set; } = maxY;
|
||||
|
||||
public double DistanceToPoint(double x, double y)
|
||||
{
|
||||
double dx = Math.Max(Math.Max(MinX - x, 0), x - MaxX);
|
||||
double dy = Math.Max(Math.Max(MinY - y, 0), y - MaxY);
|
||||
return Math.Sqrt(dx * dx + dy * dy);
|
||||
}
|
||||
|
||||
public bool Contains(double x, double y)
|
||||
{
|
||||
return x >= MinX && x <= MaxX && y >= MinY && y <= MaxY;
|
||||
}
|
||||
}
|
||||
|
||||
public class RTreeNode
|
||||
{
|
||||
public Rectangle Bounds { get; set; } = new Rectangle(0, 0, 0, 0);
|
||||
public List<RTreeNode> Children { get; set; } = [];
|
||||
public List<(EdgeDto Edge, Rectangle Rect)> Entries { get; set; } = [];
|
||||
public bool IsLeaf => Children.Count == 0;
|
||||
}
|
||||
|
||||
public class RTree
|
||||
{
|
||||
private readonly RTreeNode Root;
|
||||
private const int MaxEntries = 4;
|
||||
private readonly List<NodeDto> Nodes;
|
||||
|
||||
public RTree(List<NodeDto> nodes, List<EdgeDto> edges)
|
||||
{
|
||||
Nodes = nodes;
|
||||
Root = new RTreeNode();
|
||||
foreach (var edge in edges)
|
||||
{
|
||||
var rect = CalculateMBR(edge);
|
||||
Insert(Root, (edge, rect));
|
||||
}
|
||||
}
|
||||
|
||||
private Rectangle CalculateMBR(EdgeDto edge)
|
||||
{
|
||||
var startNode = Nodes.FirstOrDefault(n => n.Id == edge.StartNodeId);
|
||||
var endNode = Nodes.FirstOrDefault(n => n.Id == edge.EndNodeId);
|
||||
if (startNode == null || endNode == null) return new Rectangle(0, 0, 0, 0);
|
||||
|
||||
double minX = Math.Min(startNode.X, endNode.X);
|
||||
double maxX = Math.Max(startNode.X, endNode.X);
|
||||
double minY = Math.Min(startNode.Y, endNode.Y);
|
||||
double maxY = Math.Max(startNode.Y, endNode.Y);
|
||||
|
||||
// Mở rộng MBR nếu edge là đường cong
|
||||
if (edge.TrajectoryDegree != TrajectoryDegree.One)
|
||||
{
|
||||
minX = Math.Min(minX, Math.Min(edge.ControlPoint1X, edge.ControlPoint2X));
|
||||
maxX = Math.Max(maxX, Math.Max(edge.ControlPoint1X, edge.ControlPoint2X));
|
||||
minY = Math.Min(minY, Math.Min(edge.ControlPoint1Y, edge.ControlPoint2Y));
|
||||
maxY = Math.Max(maxY, Math.Max(edge.ControlPoint1Y, edge.ControlPoint2Y));
|
||||
}
|
||||
|
||||
return new Rectangle(minX, minY, maxX, maxY);
|
||||
}
|
||||
|
||||
private static void Insert(RTreeNode node, (EdgeDto Edge, Rectangle Rect) entry)
|
||||
{
|
||||
if (node.IsLeaf)
|
||||
{
|
||||
node.Entries.Add(entry);
|
||||
if (node.Entries.Count > MaxEntries)
|
||||
{
|
||||
SplitNode(node);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
var bestChild = ChooseBestChild(node, entry.Rect);
|
||||
Insert(bestChild, entry);
|
||||
AdjustBounds(bestChild);
|
||||
}
|
||||
|
||||
node.Bounds.MinX = Math.Min(node.Bounds.MinX, entry.Rect.MinX);
|
||||
node.Bounds.MinY = Math.Min(node.Bounds.MinY, entry.Rect.MinY);
|
||||
node.Bounds.MaxX = Math.Max(node.Bounds.MaxX, entry.Rect.MaxX);
|
||||
node.Bounds.MaxY = Math.Max(node.Bounds.MaxY, entry.Rect.MaxY);
|
||||
}
|
||||
|
||||
private static RTreeNode ChooseBestChild(RTreeNode node, Rectangle rect)
|
||||
{
|
||||
RTreeNode best = node.Children[0];
|
||||
double minEnlargement = CalculateEnlargement(best.Bounds, rect);
|
||||
foreach (var child in node.Children.Skip(1))
|
||||
{
|
||||
double enlargement = CalculateEnlargement(child.Bounds, rect);
|
||||
if (enlargement < minEnlargement)
|
||||
{
|
||||
minEnlargement = enlargement;
|
||||
best = child;
|
||||
}
|
||||
}
|
||||
return best;
|
||||
}
|
||||
|
||||
private static double CalculateEnlargement(Rectangle bounds, Rectangle rect)
|
||||
{
|
||||
double newArea = (Math.Max(bounds.MaxX, rect.MaxX) - Math.Min(bounds.MinX, rect.MinX)) *
|
||||
(Math.Max(bounds.MaxY, rect.MaxY) - Math.Min(bounds.MinY, rect.MinY));
|
||||
double oldArea = (bounds.MaxX - bounds.MinX) * (bounds.MaxY - bounds.MinY);
|
||||
return newArea - oldArea;
|
||||
}
|
||||
|
||||
private static void SplitNode(RTreeNode node)
|
||||
{
|
||||
var (group1, group2) = SplitEntries(node.Entries);
|
||||
node.Children.Add(new RTreeNode { Entries = group1 });
|
||||
node.Children.Add(new RTreeNode { Entries = group2 });
|
||||
node.Entries.Clear();
|
||||
foreach (var child in node.Children)
|
||||
{
|
||||
child.Bounds = CalculateBounds(child.Entries);
|
||||
}
|
||||
}
|
||||
|
||||
private static (List<(EdgeDto, Rectangle)>, List<(EdgeDto, Rectangle)>) SplitEntries(List<(EdgeDto Edge, Rectangle Rect)> entries)
|
||||
{
|
||||
entries.Sort((a, b) => a.Rect.MinX.CompareTo(b.Rect.MinX));
|
||||
int mid = entries.Count / 2;
|
||||
return (entries.GetRange(0, mid), entries.GetRange(mid, entries.Count - mid));
|
||||
}
|
||||
|
||||
private static Rectangle CalculateBounds(List<(EdgeDto Edge, Rectangle Rect)> entries)
|
||||
{
|
||||
if (entries.Count == 0) return new(0, 0, 0, 0);
|
||||
var first = entries[0].Rect;
|
||||
double minX = first.MinX, minY = first.MinY, maxX = first.MaxX, maxY = first.MaxY;
|
||||
foreach (var (Edge, Rect) in entries.Skip(1))
|
||||
{
|
||||
minX = Math.Min(minX, Rect.MinX);
|
||||
minY = Math.Min(minY, Rect.MinY);
|
||||
maxX = Math.Max(maxX, Rect.MaxX);
|
||||
maxY = Math.Max(maxY, Rect.MaxY);
|
||||
}
|
||||
return new Rectangle(minX, minY, maxX, maxY);
|
||||
}
|
||||
|
||||
private static void AdjustBounds(RTreeNode node)
|
||||
{
|
||||
if (node.IsLeaf)
|
||||
{
|
||||
node.Bounds = CalculateBounds(node.Entries);
|
||||
}
|
||||
else
|
||||
{
|
||||
node.Bounds = CalculateBounds(node.Children.SelectMany(c => c.Entries).ToList());
|
||||
}
|
||||
}
|
||||
|
||||
public EdgeDto? FindNearest(double x, double y, double limitDistance)
|
||||
{
|
||||
double minDistance = double.MaxValue;
|
||||
EdgeDto? nearestEdge = null;
|
||||
SearchNearest(Root, x, y, ref minDistance, ref nearestEdge);
|
||||
return minDistance <= limitDistance ? nearestEdge : null;
|
||||
}
|
||||
|
||||
private void SearchNearest(RTreeNode node, double x, double y, ref double minDistance, ref EdgeDto? nearestEdge)
|
||||
{
|
||||
if (node.IsLeaf)
|
||||
{
|
||||
foreach (var (edge, rect) in node.Entries)
|
||||
{
|
||||
var startNode = Nodes.FirstOrDefault(n => n.Id == edge.StartNodeId);
|
||||
var endNode = Nodes.FirstOrDefault(n => n.Id == edge.EndNodeId);
|
||||
if (startNode == null || endNode == null) continue;
|
||||
|
||||
var distanceResult = MapCompute.DistanceToEdge(x, y, edge, startNode, endNode);
|
||||
if (distanceResult.IsSuccess && distanceResult.Data < minDistance)
|
||||
{
|
||||
minDistance = distanceResult.Data;
|
||||
nearestEdge = edge;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
var sortedChildren = node.Children.OrderBy(c => c.Bounds.DistanceToPoint(x, y));
|
||||
foreach (var child in sortedChildren)
|
||||
{
|
||||
if (child.Bounds.DistanceToPoint(x, y) < minDistance)
|
||||
{
|
||||
SearchNearest(child, x, y, ref minDistance, ref nearestEdge);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user