first commit -push
This commit is contained in:
dungtt
@@ -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;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user