472 lines
25 KiB
C#
472 lines
25 KiB
C#
using RobotNet10.GlobalPathPlanner.AStar;
|
|
using RobotNet10.GlobalPathPlanner.Model;
|
|
using System;
|
|
using System.IO;
|
|
|
|
namespace RobotNet10.GlobalPathPlanner.Forklift;
|
|
|
|
public class ForkliftPathPlanner : IPathPlanner
|
|
{
|
|
private List<GlobalNode> Nodes = [];
|
|
private List<GlobalEdge> Edges = [];
|
|
private const double Ratio = 0.1;
|
|
|
|
private PathPlannerOptions Options = new()
|
|
{
|
|
LimitDistanceToEdge = 1,
|
|
LimitDistanceToNode = 0.3,
|
|
ResolutionSplit = 0.1,
|
|
ChangeOrientationAngle = 89
|
|
};
|
|
public void SetData(GlobalNode[] nodes, GlobalEdge[] edges)
|
|
{
|
|
Nodes = [.. nodes];
|
|
Edges = [.. edges];
|
|
}
|
|
|
|
public void SetOptions(PathPlannerOptions options)
|
|
{
|
|
Options = options;
|
|
}
|
|
|
|
private static bool TStructureExisted(List<TStructure> TStructures, GlobalNode node1, GlobalNode node2, GlobalNode 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 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<GlobalNode> 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.Degree == 1 &&
|
|
edge1.Degree == 1 &&
|
|
edge2.Degree == 1) continue;
|
|
|
|
TStructures.Add(new()
|
|
{
|
|
Node1 = node,
|
|
Node2 = inNodes[i],
|
|
Node3 = inNodes[j],
|
|
Edge12 = edge1,
|
|
Edge13 = edge2,
|
|
Edge23 = edgeT,
|
|
});
|
|
}
|
|
}
|
|
}
|
|
return [.. TStructures];
|
|
}
|
|
|
|
private (bool IsSuccess, GlobalNode? intraNode, TStructure? tstructure) IsReverse(GlobalNode currentNode, GlobalNode olderNode, GlobalNode? futureNode, GlobalEdge olderedge, GlobalEdge? 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, Options.ChangeOrientationAngle);
|
|
var intraNode = ts.GetIntraNode(currentNode, olderNode);
|
|
if (intraNode is null) continue;
|
|
|
|
if (!ts.IsAccessDirection(olderNode, intraNode) || !ts.IsAccessDirection(intraNode, currentNode)) continue;
|
|
|
|
var currentDirection = MathExtensions.GetOrientationStart(olderNode, currentNode, olderedge, startAngle, Ratio, Options.ChangeOrientationAngle);
|
|
var intraEdge = ts.GetEdge(olderNode, intraNode);
|
|
if (intraEdge is null) continue;
|
|
var branchDirection = MathExtensions.GetOrientationStart(olderNode, intraNode, intraEdge, startAngle, Ratio, Options.ChangeOrientationAngle);
|
|
bool firstReverse = branchDirection != currentDirection;
|
|
|
|
bool endReverse = false;
|
|
if (futureNode is not null && futureedge is not null)
|
|
{
|
|
startAngle = MathExtensions.GetEndAngle(olderNode, currentNode, olderedge, 1 - Ratio);
|
|
currentDirection = MathExtensions.GetOrientationEnd(currentNode, futureNode, futureedge, startAngle, Ratio, Options.ChangeOrientationAngle);
|
|
intraEdge = ts.GetEdge(currentNode, intraNode);
|
|
if (intraEdge is null) continue;
|
|
startAngle = MathExtensions.GetEndAngle(intraNode, currentNode, intraEdge, 1 - Ratio);
|
|
branchDirection = MathExtensions.GetOrientationEnd(currentNode, futureNode, futureedge, startAngle, Ratio, Options.ChangeOrientationAngle);
|
|
endReverse = branchDirection != 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<GlobalNode> GetIntermediateNode(GlobalNode startNode, GlobalNode endNode)
|
|
{
|
|
var edge1s = Edges.Where(e => e.StartNodeId == startNode.Id).ToList();
|
|
var edge2s = Edges.Where(e => e.StartNodeId == endNode.Id).ToList();
|
|
if (edge1s is null || edge2s is null || edge1s.Count < 2 || edge2s.Count < 2) return [];
|
|
List<GlobalNode> node1 = [];
|
|
List<GlobalNode> IntermediateNode = [];
|
|
foreach (var edge1 in edge1s)
|
|
{
|
|
if (edge1.Degree != 1) continue;
|
|
if (edge1.EndNodeId == endNode.Id) continue;
|
|
var interNode = Nodes.FirstOrDefault(n => n.Id == edge1.EndNodeId);
|
|
if (interNode is null) continue;
|
|
node1.Add(interNode);
|
|
}
|
|
if (node1.Count == 0) return [];
|
|
foreach (var edge2 in edge2s)
|
|
{
|
|
if (edge2.Degree != 1) continue;
|
|
if (edge2.EndNodeId == startNode.Id) continue;
|
|
var interNode = Nodes.FirstOrDefault(n => n.Id == edge2.EndNodeId);
|
|
if (interNode is null) continue;
|
|
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 (GlobalNode[] NodesFilter, GlobalEdge[] EdgesFilter) FilterPathPlanning(GlobalNode[] nodes, GlobalEdge[] edges, GlobalEdge? closesEdge)
|
|
{
|
|
if (nodes.Length <= 1 || edges.Length < 1 || nodes.Length - 1 != edges.Length) return ([], []);
|
|
List<GlobalNode> 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]);
|
|
}
|
|
if (nodeFilter[^1].Id != nodes[^1].Id)
|
|
nodeFilter.Add(nodes[^1]);
|
|
var edgeFilter = MathExtensions.GetEdgesPlanning([.. nodeFilter], [.. Edges], closesEdge);
|
|
if (nodeFilter.Count - 1 != edgeFilter.Length) return ([], []);
|
|
return ([.. nodeFilter], [.. edgeFilter]);
|
|
}
|
|
|
|
private (GlobalNode[] Nodes, GlobalEdge[] Edges) CheckPathWithFinalDirection(GlobalNode[] nodes, GlobalEdge[] edges, double currentAngle, Orientation goalDirection = Orientation.NONE)
|
|
{
|
|
if ((nodes[^1].Orientation == goalDirection && MathExtensions.GetEdgesLength([.. edges], [.. Nodes]) < 10) || goalDirection == Orientation.NONE)
|
|
return FilterPathPlanning([.. nodes], [.. edges], null);
|
|
|
|
var edgeplannings = edges.ToList();
|
|
var nodeplannings = nodes.ToList();
|
|
|
|
var TStructures = GetTStructure();
|
|
|
|
Guid LastReverseDirectionId = Guid.Empty;
|
|
GlobalNode LastNodeReverseDirection = new();
|
|
for (int i = 1; i < nodeplannings.Count; i++)
|
|
{
|
|
if (nodeplannings[i].Orientation == Orientation.FORWARD) continue;
|
|
GlobalNode? futureNode = null;
|
|
GlobalEdge? futureEdge = null;
|
|
if (i < nodeplannings.Count - 1)
|
|
{
|
|
futureNode = nodeplannings[i + 1];
|
|
futureEdge = edgeplannings[i];
|
|
}
|
|
double startAngle = currentAngle;
|
|
if (i >= 2) startAngle = MathExtensions.GetEndAngle(nodeplannings[i - 2], nodeplannings[i - 1], edgeplannings[i - 2], Ratio);
|
|
(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,
|
|
Degree = edge1.Degree,
|
|
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,
|
|
Degree = edge2.Degree,
|
|
ControlPoint1X = edge2.ControlPoint1X,
|
|
ControlPoint1Y = edge2.ControlPoint1Y,
|
|
ControlPoint2X = edge2.ControlPoint2X,
|
|
ControlPoint2Y = edge2.ControlPoint2Y
|
|
});
|
|
nodeplannings.Insert(i, intraNode);
|
|
var directionInPath = MathExtensions.GetOrientations(nodeplannings[0].Orientation, [.. nodeplannings], [.. edgeplannings], Ratio, Options.ChangeOrientationAngle);
|
|
for (int j = 0; j < nodeplannings.Count; j++)
|
|
{
|
|
nodeplannings[j].Orientation = directionInPath[j];
|
|
}
|
|
LastReverseDirectionId = tstructure.Id;
|
|
LastNodeReverseDirection = nodeplannings[i + 1];
|
|
i++;
|
|
}
|
|
|
|
if (nodeplannings[^1].Orientation == goalDirection) return FilterPathPlanning([.. nodeplannings], [.. edgeplannings], null);
|
|
|
|
for (int i = nodeplannings.Count - 1; i > 0; i--)
|
|
{
|
|
GlobalNode? futureNode = null;
|
|
GlobalEdge? futureEdge = null;
|
|
if (i < nodeplannings.Count - 1)
|
|
{
|
|
futureNode = nodeplannings[i + 1];
|
|
futureEdge = edgeplannings[i];
|
|
}
|
|
double startAngle = currentAngle;
|
|
if (i >= 2) startAngle = MathExtensions.GetEndAngle(nodeplannings[i - 2], nodeplannings[i - 1], edgeplannings[i - 2], Ratio);
|
|
(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,
|
|
Degree = edge.Degree,
|
|
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,
|
|
Degree = edge1.Degree,
|
|
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,
|
|
Degree = edge2.Degree,
|
|
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,
|
|
Degree = edge.Degree,
|
|
ControlPoint1X = edge.ControlPoint1X,
|
|
ControlPoint1Y = edge.ControlPoint1Y,
|
|
ControlPoint2X = edge.ControlPoint2X,
|
|
ControlPoint2Y = edge.ControlPoint2Y,
|
|
});
|
|
edgeplannings.RemoveAt(i);
|
|
edgeplannings.RemoveAt(i - 1);
|
|
nodeplannings.RemoveAt(i - 1);
|
|
}
|
|
var directionInPath = MathExtensions.GetOrientations(nodeplannings[0].Orientation, [.. nodeplannings], [.. edgeplannings], Ratio, Options.ChangeOrientationAngle);
|
|
if (directionInPath[^1] == goalDirection)
|
|
{
|
|
for (int j = 0; j < nodeplannings.Count; j++)
|
|
{
|
|
nodeplannings[j].Orientation = directionInPath[j];
|
|
}
|
|
return FilterPathPlanning([.. nodeplannings], [.. edgeplannings], null);
|
|
}
|
|
}
|
|
throw new Exception("The path to the destination does not satisfy the conditions");
|
|
}
|
|
|
|
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanning(double x, double y, double theta, Guid goalId, CancellationToken? cancellationToken = null)
|
|
{
|
|
var goal = Nodes.FirstOrDefault(n => n.Id == goalId) ?? throw new Exception($"Destination {goalId} does not exist in the map");
|
|
|
|
using CancellationTokenSource cancellation = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken ?? CancellationToken.None);
|
|
if (Options.TimeOut != null && Options.TimeOut.HasValue) cancellation.CancelAfter(Options.TimeOut.Value);
|
|
|
|
try
|
|
{
|
|
var AStarPathPlanner = new AStarPlanner(Nodes, Edges);
|
|
(var path, var closesEdge) = AStarPathPlanner.Planning(x,
|
|
y,
|
|
new GlobalNode() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
|
|
Options.LimitDistanceToEdge,
|
|
Options.LimitDistanceToNode,
|
|
cancellation.Token);
|
|
|
|
if (path is null || path.Length < 1) throw new Exception($"The path to {goal.Name} - {goal.Id} does not exist from [{x}, {y}, {theta}]");
|
|
if (path.Length == 1) return (path, []);
|
|
|
|
var edgeplannings = MathExtensions.GetEdgesPlanning([.. path], [.. Edges], closesEdge);
|
|
|
|
Orientation CurrenDirection = MathExtensions.GetOrientationStart(path[0], path[1], edgeplannings[0], theta, Ratio, Options.ChangeOrientationAngle);
|
|
var FinalDirection = MathExtensions.GetOrientations(CurrenDirection, path, edgeplannings, Ratio, Options.ChangeOrientationAngle);
|
|
for (int i = 0; i < path.Length; i++)
|
|
{
|
|
path[i].Orientation = FinalDirection[i];
|
|
}
|
|
|
|
return ([.. path], [.. edgeplannings]);
|
|
}
|
|
catch (OperationCanceledException)
|
|
{
|
|
if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) throw new OperationCanceledException();
|
|
else throw new TimeoutException();
|
|
}
|
|
}
|
|
|
|
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithStartDirection(double x, double y, double theta, Guid goalId, Orientation startDiretion = Orientation.NONE, CancellationToken? cancellationToken = null)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithFinalDirection(double x, double y, double theta, Guid goalId, Orientation goalDirection = Orientation.NONE, CancellationToken? cancellationToken = null)
|
|
{
|
|
var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
|
|
if (basicPath.Nodes.Length < 1) return basicPath;
|
|
|
|
return CheckPathWithFinalDirection(basicPath.Nodes, basicPath.Edges, theta, goalDirection);
|
|
}
|
|
|
|
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithAngle(double x, double y, double theta, Guid goalId, double goalAngle, CancellationToken? cancellationToken = null)
|
|
{
|
|
var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
|
|
if (basicPath.Nodes.Length < 1) return basicPath;
|
|
|
|
Orientation goalDirection = MathExtensions.GetOrientationEnd(basicPath.Nodes[^1], basicPath.Nodes[^2], basicPath.Edges[^1], goalAngle, Ratio, Options.ChangeOrientationAngle);
|
|
|
|
return CheckPathWithFinalDirection(basicPath.Nodes, basicPath.Edges, theta, goalDirection);
|
|
}
|
|
|
|
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanning(Guid startNodeId, double theta, Guid goalId, CancellationToken? cancellationToken = null)
|
|
{
|
|
var goal = Nodes.FirstOrDefault(n => n.Id == goalId) ?? throw new Exception($"Destination {goalId} does not exist in the map");
|
|
var startNode = Nodes.FirstOrDefault(n => n.Id == startNodeId) ?? throw new Exception($"Starting node {startNodeId} does not exist in the map");
|
|
|
|
using CancellationTokenSource cancellation = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken ?? CancellationToken.None);
|
|
if (Options.TimeOut != null && Options.TimeOut.HasValue) cancellation.CancelAfter(Options.TimeOut.Value);
|
|
try
|
|
{
|
|
var AStarPathPlanner = new AStarPlanner(Nodes, Edges);
|
|
(var path, var closesEdge) = AStarPathPlanner.Planning(startNode, goal, cancellation.Token);
|
|
|
|
if (path is null || path.Length < 1) throw new Exception($"The path to {goal.Name} - {goal.Id} does not exist from [{startNode.Name} - {startNode.Id}]");
|
|
if (path.Length == 1) return (path, []);
|
|
|
|
var edgeplannings = MathExtensions.GetEdgesPlanning([.. path], [.. Edges], closesEdge);
|
|
|
|
Orientation CurrenDirection = MathExtensions.GetOrientationStart(path[0], path[1], edgeplannings[0], theta, Ratio, Options.ChangeOrientationAngle);
|
|
var FinalDirection = MathExtensions.GetOrientations(CurrenDirection, path, edgeplannings, Ratio, Options.ChangeOrientationAngle);
|
|
for (int i = 0; i < path.Length; i++)
|
|
{
|
|
path[i].Orientation = FinalDirection[i];
|
|
}
|
|
|
|
return ([.. path], [.. edgeplannings]);
|
|
}
|
|
catch (OperationCanceledException)
|
|
{
|
|
if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) throw new OperationCanceledException();
|
|
else throw new TimeoutException();
|
|
}
|
|
}
|
|
|
|
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithStartDirection(Guid startNodeId, double theta, Guid goalId, Orientation startDiretion = Orientation.NONE, CancellationToken? cancellationToken = null)
|
|
{
|
|
throw new NotImplementedException();
|
|
}
|
|
|
|
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithFinalDirection(Guid startNodeId, double theta, Guid goalId, Orientation goalDirection = Orientation.NONE, CancellationToken? cancellationToken = null)
|
|
{
|
|
var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
|
|
if (basicPath.Nodes.Length < 1) return basicPath;
|
|
|
|
return CheckPathWithFinalDirection(basicPath.Nodes, basicPath.Edges, theta, goalDirection);
|
|
}
|
|
|
|
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithAngle(Guid startNodeId, double theta, Guid goalId, double goalAngle, CancellationToken? cancellationToken = null)
|
|
{
|
|
var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
|
|
if (basicPath.Nodes.Length < 1) return basicPath;
|
|
|
|
Orientation goalDirection = MathExtensions.GetOrientationEnd(basicPath.Nodes[^1], basicPath.Nodes[^2], basicPath.Edges[^1], goalAngle, Ratio, Options.ChangeOrientationAngle);
|
|
|
|
return CheckPathWithFinalDirection(basicPath.Nodes, basicPath.Edges, theta, goalDirection);
|
|
}
|
|
}
|