622 lines
29 KiB
C#
622 lines
29 KiB
C#
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] };
|
|
}
|
|
}
|