Initial commit
This commit is contained in:
@@ -0,0 +1,331 @@
|
||||
using RobotNet10.GlobalPathPlanner.AStar;
|
||||
using RobotNet10.GlobalPathPlanner.Forklift;
|
||||
using RobotNet10.GlobalPathPlanner.Model;
|
||||
|
||||
namespace RobotNet10.GlobalPathPlanner.ForkliftV2;
|
||||
|
||||
public class ForkLiftPathPlannerV2 : 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;
|
||||
}
|
||||
|
||||
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 SSEAStarPlanner = new SSEAStarPlanner(Nodes, Edges, Options);
|
||||
(var path, var closesEdge) = SSEAStarPlanner.PlanningWithFinalDirection(x, y, theta,
|
||||
new GlobalNode() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y},
|
||||
Orientation.NONE,
|
||||
Options.LimitDistanceToEdge,
|
||||
Options.LimitDistanceToNode,
|
||||
cancellation.Token);
|
||||
|
||||
if (path is null || path.Length < 1)
|
||||
{
|
||||
var ForkliftV1 = new ForkliftPathPlanner();
|
||||
ForkliftV1.SetData([.. Nodes], [.. Edges]);
|
||||
return ForkliftV1.PathPlanning(x,y,theta,goal.Id, cancellation.Token);
|
||||
}
|
||||
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] = new GlobalNode
|
||||
{
|
||||
Id = path[i].Id,
|
||||
X = path[i].X,
|
||||
Y = path[i].Y,
|
||||
Name = path[i].Name,
|
||||
MapId = path[i].MapId,
|
||||
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 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 SSEAStarPlanner = new SSEAStarPlanner(Nodes, Edges, Options);
|
||||
(var path, var closesEdge) = SSEAStarPlanner.PlanningWithFinalDirection(x, y, theta,
|
||||
new GlobalNode() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
|
||||
goalDirection,
|
||||
Options.LimitDistanceToEdge,
|
||||
Options.LimitDistanceToNode,
|
||||
cancellation.Token);
|
||||
|
||||
if (path is null || path.Length < 1)
|
||||
{
|
||||
var ForkliftV1 = new ForkliftPathPlanner();
|
||||
ForkliftV1.SetData([.. Nodes], [.. Edges]);
|
||||
return ForkliftV1.PathPlanningWithFinalDirection(x, y, theta, goal.Id, goalDirection, cancellation.Token);
|
||||
}
|
||||
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] = new GlobalNode
|
||||
{
|
||||
Id = path[i].Id,
|
||||
X = path[i].X,
|
||||
Y = path[i].Y,
|
||||
Name = path[i].Name,
|
||||
MapId = path[i].MapId,
|
||||
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) PathPlanningWithAngle(double x, double y, double theta, Guid goalId, double goalAngle, 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 SSEAStarPlanner = new SSEAStarPlanner(Nodes, Edges, Options);
|
||||
(var path, var closesEdge) = SSEAStarPlanner.PlanningWithGoalAngle(x, y, theta,
|
||||
new GlobalNode() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
|
||||
goalAngle,
|
||||
Options.LimitDistanceToEdge,
|
||||
Options.LimitDistanceToNode,
|
||||
cancellation.Token);
|
||||
|
||||
if (path is null || path.Length < 1)
|
||||
{
|
||||
var ForkliftV1 = new ForkliftPathPlanner();
|
||||
ForkliftV1.SetData([.. Nodes], [.. Edges]);
|
||||
return ForkliftV1.PathPlanningWithAngle(x, y, theta, goal.Id, goalAngle, cancellation.Token);
|
||||
}
|
||||
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] = new GlobalNode
|
||||
{
|
||||
Id = path[i].Id,
|
||||
X = path[i].X,
|
||||
Y = path[i].Y,
|
||||
Name = path[i].Name,
|
||||
MapId = path[i].MapId,
|
||||
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) 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 SSEAStarPlanner = new SSEAStarPlanner(Nodes, Edges, Options);
|
||||
(var path, var closesEdge) = SSEAStarPlanner.PlanningWithFinalDirection(startNode, theta,
|
||||
new GlobalNode() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
|
||||
Orientation.NONE,
|
||||
cancellation.Token);
|
||||
|
||||
if (path is null || path.Length < 1)
|
||||
{
|
||||
var ForkliftV1 = new ForkliftPathPlanner();
|
||||
ForkliftV1.SetData([.. Nodes], [.. Edges]);
|
||||
return ForkliftV1.PathPlanningWithFinalDirection(startNodeId, theta, goal.Id, Orientation.NONE, cancellation.Token);
|
||||
}
|
||||
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] = new GlobalNode
|
||||
{
|
||||
Id = path[i].Id,
|
||||
X = path[i].X,
|
||||
Y = path[i].Y,
|
||||
Name = path[i].Name,
|
||||
MapId = path[i].MapId,
|
||||
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 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 SSEAStarPlanner = new SSEAStarPlanner(Nodes, Edges, Options);
|
||||
(var path, var closesEdge) = SSEAStarPlanner.PlanningWithFinalDirection(startNode, theta,
|
||||
new GlobalNode() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
|
||||
goalDirection,
|
||||
cancellation.Token);
|
||||
|
||||
if (path is null || path.Length < 1)
|
||||
{
|
||||
var ForkliftV1 = new ForkliftPathPlanner();
|
||||
ForkliftV1.SetData([.. Nodes], [.. Edges]);
|
||||
return ForkliftV1.PathPlanningWithFinalDirection(startNodeId, theta, goal.Id, goalDirection, cancellation.Token);
|
||||
}
|
||||
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] = new GlobalNode
|
||||
{
|
||||
Id = path[i].Id,
|
||||
X = path[i].X,
|
||||
Y = path[i].Y,
|
||||
Name = path[i].Name,
|
||||
MapId = path[i].MapId,
|
||||
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) PathPlanningWithAngle(Guid startNodeId, double theta, Guid goalId, double goalAngle, 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 SSEAStarPlanner = new SSEAStarPlanner(Nodes, Edges, Options);
|
||||
(var path, var closesEdge) = SSEAStarPlanner.PlanningWithGoalAngle(startNode, theta,
|
||||
new GlobalNode() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
|
||||
goalAngle,
|
||||
cancellation.Token);
|
||||
|
||||
if (path is null || path.Length < 1)
|
||||
{
|
||||
var ForkliftV1 = new ForkliftPathPlanner();
|
||||
ForkliftV1.SetData([.. Nodes], [.. Edges]);
|
||||
return ForkliftV1.PathPlanningWithAngle(startNodeId, theta, goal.Id, goalAngle, cancellation.Token);
|
||||
}
|
||||
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] = new GlobalNode
|
||||
{
|
||||
Id = path[i].Id,
|
||||
X = path[i].X,
|
||||
Y = path[i].Y,
|
||||
Name = path[i].Name,
|
||||
MapId = path[i].MapId,
|
||||
Orientation = FinalDirection[i]
|
||||
};
|
||||
}
|
||||
|
||||
return ([.. path], [.. edgeplannings]);
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) throw new OperationCanceledException();
|
||||
else throw new TimeoutException();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,567 @@
|
||||
using RobotNet10.GlobalPathPlanner.Model;
|
||||
using RobotNet10.GlobalPathPlanner.Space;
|
||||
|
||||
namespace RobotNet10.GlobalPathPlanner.ForkliftV2;
|
||||
|
||||
public class SSEAStarPlanner(List<GlobalNode> Nodes, List<GlobalEdge> Edges, PathPlannerOptions Options)
|
||||
{
|
||||
private const double Ratio = 0.01;
|
||||
private GlobalNode? 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 GlobalEdge[]? GetClosesEdges(GlobalNode nodeRef, double limitDistance)
|
||||
{
|
||||
double minDistance = double.MaxValue;
|
||||
List<GlobalEdge> edgesResult = [];
|
||||
foreach (var edge in Edges)
|
||||
{
|
||||
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 distance = MathExtensions.DistanceToEdge(nodeRef, startNode, endNode, edge);
|
||||
if (distance < minDistance)
|
||||
{
|
||||
minDistance = distance;
|
||||
edgesResult = [edge];
|
||||
var reverseEdge = Edges.FirstOrDefault(e => e.EndNodeId == startNode.Id && e.StartNodeId == endNode.Id);
|
||||
if (reverseEdge != null) edgesResult = [.. edgesResult, reverseEdge];
|
||||
}
|
||||
}
|
||||
if (minDistance <= limitDistance) return [.. edgesResult];
|
||||
else return null;
|
||||
}
|
||||
|
||||
public List<GlobalNode> GetNegativeNodes(Guid nodeId)
|
||||
{
|
||||
var node = Nodes.FirstOrDefault(p => p.Id == nodeId);
|
||||
if (node is null) return [];
|
||||
|
||||
var listNodesNegative = new List<GlobalNode>();
|
||||
var listPaths = Edges.Where(p => p.StartNodeId == nodeId);
|
||||
foreach (var path in listPaths)
|
||||
{
|
||||
var negativeNode = Nodes.FirstOrDefault(p => p.Id == path.EndNodeId);
|
||||
if (negativeNode != null) listNodesNegative.Add(negativeNode);
|
||||
}
|
||||
return listNodesNegative;
|
||||
}
|
||||
|
||||
private double GetNegativeCost(SSEAStarNode currenNode, SSEAStarNode 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 = MathExtensions.GetEdgeLength(startNode, endNode, edge);
|
||||
if (distance < minDistance) minDistance = distance;
|
||||
}
|
||||
return minDistance != double.MaxValue ? minDistance : 0;
|
||||
}
|
||||
|
||||
private List<SSEAStarNode> GetNegativeAStarNodes(SSEAStarNode nodeCurrent, GlobalNode endNode)
|
||||
{
|
||||
var possiblePointNegative = new List<SSEAStarNode>();
|
||||
if (nodeCurrent.Id == endNode.Id) return possiblePointNegative;
|
||||
|
||||
var listNodesNegative = GetNegativeNodes(nodeCurrent.Id);
|
||||
|
||||
foreach (var negativeNode in listNodesNegative)
|
||||
{
|
||||
if (nodeCurrent.Parent is null) continue;
|
||||
var nodeDtoCurrent = Nodes.FirstOrDefault(n => n.Id == nodeCurrent.Id);
|
||||
var nodeDtoNegative = Nodes.FirstOrDefault(n => n.Id == negativeNode.Id);
|
||||
var nodeDtoParent = Nodes.FirstOrDefault(n => n.Id == nodeCurrent.Parent.Id);
|
||||
var negativeEdge = Edges.FirstOrDefault(e => e.StartNodeId == nodeCurrent.Id && e.EndNodeId == negativeNode.Id);
|
||||
var parentEdge = Edges.FirstOrDefault(e => e.EndNodeId == nodeCurrent.Id && e.StartNodeId == nodeCurrent.Parent.Id);
|
||||
|
||||
if (nodeDtoCurrent is null || nodeDtoNegative is null || negativeEdge is null) continue;
|
||||
|
||||
var nearNodeNevgative = MathExtensions.BezierPoint(Ratio, nodeDtoCurrent, nodeDtoNegative, negativeEdge);
|
||||
var nearNodeParent = nodeDtoParent is not null && parentEdge is not null ? MathExtensions.BezierPoint(Ratio, nodeDtoParent, nodeDtoCurrent, parentEdge) :
|
||||
new()
|
||||
{
|
||||
Id = nodeCurrent.Parent.Id,
|
||||
X = nodeCurrent.Parent.X,
|
||||
Y = nodeCurrent.Parent.Y,
|
||||
Name = nodeCurrent.Parent.Name
|
||||
};
|
||||
|
||||
var angle = MathExtensions.GetAngle(nodeDtoCurrent, nearNodeNevgative, nearNodeParent);
|
||||
Orientation orientation = angle < Options.ChangeOrientationAngle ? nodeCurrent.Orientation == Orientation.FORWARD ? Orientation.BACKWARD : Orientation.FORWARD : nodeCurrent.Orientation;
|
||||
|
||||
var nodeNegative = new SSEAStarNode
|
||||
{
|
||||
Id = negativeNode.Id,
|
||||
X = negativeNode.X,
|
||||
Y = negativeNode.Y,
|
||||
Name = negativeNode.Name,
|
||||
Orientation = orientation,
|
||||
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 + (orientation == Orientation.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 * (1 + (orientation == Orientation.BACKWARD ? Math.Sqrt(2) / 2 : 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, GlobalNode goal, Orientation 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 == goal.Id)
|
||||
{
|
||||
if (checkNode.Orientation == goalDirection || goalDirection == Orientation.NONE)
|
||||
{
|
||||
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 = GetNegativeAStarNodes(checkNode, goal);
|
||||
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.Orientation == node.Orientation);
|
||||
if (activeNode is not null && activeNode.TotalCost > node.TotalCost)
|
||||
{
|
||||
activeNodes.Items.Remove(activeNode);
|
||||
activeNodes.Enqueue(node);
|
||||
}
|
||||
else if (activeNode is null)
|
||||
{
|
||||
activeNodes.Enqueue(node);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return [];
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
return [];
|
||||
}
|
||||
}
|
||||
|
||||
public List<SSEAStarNode> Find(SSEAStarNode startNode, GlobalNode goal, double goalAngle, 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 == goal.Id)
|
||||
{
|
||||
if (checkNode.Parent is not null)
|
||||
{
|
||||
var nodeParentDto = Nodes.FirstOrDefault(n => n.Id == checkNode.Parent.Id);
|
||||
var edge = Edges.FirstOrDefault(e => e.EndNodeId == checkNode.Id && e.StartNodeId == checkNode.Parent.Id);
|
||||
if (edge is not null && nodeParentDto is not null)
|
||||
{
|
||||
var nearParent = MathExtensions.BezierPoint(Ratio, nodeParentDto, goal, edge);
|
||||
var nearGoalNode = new GlobalNode()
|
||||
{
|
||||
X = goal.X + Math.Cos(goalAngle * Math.PI / 180),
|
||||
Y = goal.Y + Math.Sin(goalAngle * Math.PI / 180),
|
||||
};
|
||||
|
||||
var angle = MathExtensions.GetAngle(goal, nearParent, nearGoalNode);
|
||||
Orientation goalDirection = angle < Options.ChangeOrientationAngle ? Orientation.BACKWARD : Orientation.FORWARD;
|
||||
if (checkNode.Orientation == 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 = GetNegativeAStarNodes(checkNode, goal);
|
||||
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.Orientation == node.Orientation);
|
||||
if (activeNode is not null && activeNode.TotalCost > node.TotalCost)
|
||||
{
|
||||
activeNodes.Items.Remove(activeNode);
|
||||
activeNodes.Enqueue(node);
|
||||
}
|
||||
else if (activeNode is null)
|
||||
{
|
||||
activeNodes.Enqueue(node);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return [];
|
||||
}
|
||||
catch (OperationCanceledException)
|
||||
{
|
||||
return [];
|
||||
}
|
||||
}
|
||||
|
||||
private SSEAStarNode GetClosesNode(GlobalNode closesNode, GlobalNode goal, double theta)
|
||||
{
|
||||
SSEAStarNode closesAStarNode = new()
|
||||
{
|
||||
Id = closesNode.Id,
|
||||
X = closesNode.X,
|
||||
Y = closesNode.Y,
|
||||
Name = closesNode.Name,
|
||||
};
|
||||
foreach (var negativeNode in GetNegativeNodes(closesAStarNode.Id))
|
||||
{
|
||||
SSEAStarNode closesAStarNodeParent = new()
|
||||
{
|
||||
Id = closesNode.Id,
|
||||
X = closesNode.X,
|
||||
Y = closesNode.Y,
|
||||
Name = closesNode.Name,
|
||||
};
|
||||
var RobotNearNode = new GlobalNode()
|
||||
{
|
||||
X = closesAStarNode.X + Math.Cos(theta * Math.PI / 180),
|
||||
Y = closesAStarNode.Y + Math.Sin(theta * Math.PI / 180),
|
||||
};
|
||||
|
||||
var angle = MathExtensions.GetAngle(closesNode, negativeNode, RobotNearNode);
|
||||
Orientation orientation = angle < 91 ? Orientation.FORWARD : Orientation.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 += orientation == Orientation.BACKWARD ? cost * Math.Sqrt(2) / 2 : 0.0;
|
||||
closesAStarNodeParent.Orientation = orientation;
|
||||
closesAStarNode.NegativeNodes.Add(new()
|
||||
{
|
||||
Id = negativeNode.Id,
|
||||
X = negativeNode.X,
|
||||
Y = negativeNode.Y,
|
||||
Name = negativeNode.Name,
|
||||
Orientation = orientation,
|
||||
Cost = cost,
|
||||
Heuristic = Math.Abs(goal.X - negativeNode.X) + Math.Abs(goal.Y - negativeNode.Y),
|
||||
Parent = closesAStarNodeParent,
|
||||
});
|
||||
}
|
||||
return closesAStarNode;
|
||||
}
|
||||
|
||||
private SSEAStarNode[] GetStartNegativeNodes(GlobalEdge[] closesEdges, GlobalNode goal, SSEAStarNode robotNode, double theta)
|
||||
{
|
||||
List<SSEAStarNode> negativeNodes = [];
|
||||
foreach(var edge in closesEdges)
|
||||
{
|
||||
var endNode = Nodes.FirstOrDefault(n => n.Id == edge.EndNodeId);
|
||||
if (endNode == null) continue;
|
||||
SSEAStarNode closesAStarNodeParent = new()
|
||||
{
|
||||
Id = robotNode.Id,
|
||||
X = robotNode.X,
|
||||
Y = robotNode.Y,
|
||||
Name = robotNode.Name,
|
||||
};
|
||||
var RobotNearNode = new GlobalNode()
|
||||
{
|
||||
X = robotNode.X + Math.Cos(theta * Math.PI / 180),
|
||||
Y = robotNode.Y + Math.Sin(theta * Math.PI / 180),
|
||||
};
|
||||
var angle = MathExtensions.GetAngle(new() { X = robotNode.X, Y = robotNode.Y}, endNode, RobotNearNode);
|
||||
Orientation orientation = angle < Options.ChangeOrientationAngle ? Orientation.FORWARD : Orientation.BACKWARD;
|
||||
|
||||
double cost = Math.Sqrt(Math.Pow(robotNode.X - endNode.X, 2) + Math.Pow(robotNode.Y - endNode.Y, 2));
|
||||
cost += orientation == Orientation.BACKWARD ? cost * Math.Sqrt(2) / 2 : 0.0;
|
||||
closesAStarNodeParent.Orientation = orientation;
|
||||
negativeNodes.Add(new()
|
||||
{
|
||||
Id = endNode.Id,
|
||||
X = endNode.X,
|
||||
Y = endNode.Y,
|
||||
Name = endNode.Name,
|
||||
Orientation = orientation,
|
||||
Cost = cost,
|
||||
Heuristic = Math.Abs(goal.X - endNode.X) + Math.Abs(goal.Y - endNode.Y),
|
||||
Parent = closesAStarNodeParent,
|
||||
});
|
||||
}
|
||||
return [.. negativeNodes];
|
||||
}
|
||||
|
||||
public (GlobalNode[] pathNodes, GlobalEdge? closeEdges) PlanningWithFinalDirection(double x, double y, double theta, GlobalNode goal, Orientation goalDirection, double maxDistanceToEdge, double maxDistanceToNode, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var Path = new List<GlobalNode>();
|
||||
|
||||
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 ([goal], null);
|
||||
RobotNode = GetClosesNode(closesNode, goal, theta);
|
||||
}
|
||||
else
|
||||
{
|
||||
var closesEdges = GetClosesEdges(new() { X = x, Y = y }, maxDistanceToEdge);
|
||||
if (closesEdges is null || closesEdges.Length == 0) throw new Exception("The robot is too far from the route");
|
||||
|
||||
var edgeToGoal = closesEdges.FirstOrDefault(e => e.EndNodeId == goal.Id);
|
||||
if (edgeToGoal != null)
|
||||
{
|
||||
var endNode = Nodes.FirstOrDefault(n => n.Id == edgeToGoal.EndNodeId);
|
||||
if (endNode != null) return ([new() {
|
||||
Id = RobotNode.Id,
|
||||
X = RobotNode.X,
|
||||
Y = RobotNode.Y,
|
||||
Name = RobotNode.Name,
|
||||
MapId = endNode.MapId,
|
||||
}, endNode], edgeToGoal);
|
||||
}
|
||||
|
||||
RobotNode.NegativeNodes.AddRange(GetStartNegativeNodes(closesEdges, goal, RobotNode, theta));
|
||||
}
|
||||
|
||||
if (RobotNode.NegativeNodes.Count < 1) throw new Exception($"The path to {goal.Name} - {goal.Id} does not exist from [{x}, {y}]");
|
||||
|
||||
var path = Find(RobotNode, goal, goalDirection, cancellationToken);
|
||||
if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) throw new OperationCanceledException();
|
||||
if (path is null || path.Count < 1) throw new Exception($"The path to {goal.Name} - {goal.Id} does not exist from [{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,
|
||||
Orientation = node.Orientation,
|
||||
});
|
||||
continue;
|
||||
}
|
||||
var nodedb = Nodes.FirstOrDefault(p => p.Id == node.Id) ?? throw new Exception($"Map data error: Node {node.Id} does not exist in the map");
|
||||
Path.Add(new GlobalNode
|
||||
{
|
||||
Id = nodedb.Id,
|
||||
X = nodedb.X,
|
||||
Y = nodedb.Y,
|
||||
Name = nodedb.Name,
|
||||
MapId = nodedb.MapId,
|
||||
Orientation = node.Orientation
|
||||
});
|
||||
}
|
||||
var startEdge = path.Count > 1 ? Edges.FirstOrDefault(e => e.StartNodeId == path[0].Id && e.EndNodeId == path[1].Id) : null;
|
||||
return ([.. Path], startEdge);
|
||||
}
|
||||
|
||||
public (GlobalNode[] pathNodes, GlobalEdge? closeEdges) PlanningWithGoalAngle(double x, double y, double theta, GlobalNode goal, double goalAngle, double maxDistanceToEdge, double maxDistanceToNode, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var Path = new List<GlobalNode>();
|
||||
|
||||
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 ([goal], null);
|
||||
RobotNode = GetClosesNode(closesNode, goal, theta);
|
||||
}
|
||||
else
|
||||
{
|
||||
var closesEdges = GetClosesEdges(new() { X = x, Y = y }, maxDistanceToEdge);
|
||||
if (closesEdges is null || closesEdges.Length == 0) throw new Exception("The robot is too far from the route");
|
||||
|
||||
var edgeToGoal = closesEdges.FirstOrDefault(e => e.EndNodeId == goal.Id);
|
||||
if (edgeToGoal != null)
|
||||
{
|
||||
var endNode = Nodes.FirstOrDefault(n => n.Id == edgeToGoal.EndNodeId);
|
||||
if (endNode != null) return ([new() {
|
||||
Id = RobotNode.Id,
|
||||
X = RobotNode.X,
|
||||
Y = RobotNode.Y,
|
||||
Name = RobotNode.Name,
|
||||
MapId = endNode.MapId,
|
||||
}, endNode], edgeToGoal);
|
||||
}
|
||||
|
||||
RobotNode.NegativeNodes.AddRange(GetStartNegativeNodes(closesEdges, goal, RobotNode, theta));
|
||||
}
|
||||
|
||||
if (RobotNode.NegativeNodes.Count < 1) throw new Exception($"The path to {goal.Name} - {goal.Id} does not exist from [{x}, {y}]");
|
||||
|
||||
var path = Find(RobotNode, goal, goalAngle, cancellationToken);
|
||||
if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) throw new OperationCanceledException();
|
||||
if (path is null || path.Count < 1) throw new Exception($"The path to {goal.Name} - {goal.Id} does not exist from [{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,
|
||||
Orientation = node.Orientation,
|
||||
});
|
||||
continue;
|
||||
}
|
||||
var nodedb = Nodes.FirstOrDefault(p => p.Id == node.Id) ?? throw new Exception($"Map data error: Node {node.Id} does not exist in the map");
|
||||
Path.Add(new GlobalNode
|
||||
{
|
||||
Id = nodedb.Id,
|
||||
X = nodedb.X,
|
||||
Y = nodedb.Y,
|
||||
Name = nodedb.Name,
|
||||
MapId = nodedb.MapId,
|
||||
Orientation = node.Orientation
|
||||
});
|
||||
}
|
||||
var startEdge = path.Count > 1 ? Edges.FirstOrDefault(e => e.StartNodeId == path[0].Id && e.EndNodeId == path[1].Id) : null;
|
||||
return ([.. Path], startEdge);
|
||||
}
|
||||
|
||||
public (GlobalNode[] pathNodes, GlobalEdge? closeEdges) PlanningWithFinalDirection(GlobalNode startNode, double theta, GlobalNode goal, Orientation goalOrientation, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var Path = new List<GlobalNode>();
|
||||
SSEAStarNode RobotNode = GetClosesNode(startNode, goal, theta);
|
||||
var path = Find(RobotNode, goal, goalOrientation, cancellationToken);
|
||||
if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) throw new OperationCanceledException();
|
||||
if (path is null || path.Count < 1) throw new Exception($"The path to {goal.Name} - {goal.Id} does not exist from [{startNode.Name} - {startNode.Id}]");
|
||||
path.Reverse();
|
||||
foreach (var node in path)
|
||||
{
|
||||
var nodedb = Nodes.FirstOrDefault(p => p.Id == node.Id) ?? throw new Exception($"Map data error: Node {node.Id} does not exist in the map");
|
||||
Path.Add(new GlobalNode
|
||||
{
|
||||
Id = nodedb.Id,
|
||||
X = nodedb.X,
|
||||
Y = nodedb.Y,
|
||||
Name = nodedb.Name,
|
||||
MapId = nodedb.MapId,
|
||||
Orientation = node.Orientation
|
||||
});
|
||||
}
|
||||
var startEdge = path.Count > 1 ? Edges.FirstOrDefault(e => e.StartNodeId == path[0].Id && e.EndNodeId == path[1].Id) : null;
|
||||
return ([.. Path], startEdge);
|
||||
}
|
||||
|
||||
public (GlobalNode[] pathNodes, GlobalEdge? closeEdges) PlanningWithGoalAngle(GlobalNode startNode, double theta, GlobalNode goal, double goalAngle, CancellationToken? cancellationToken = null)
|
||||
{
|
||||
var Path = new List<GlobalNode>();
|
||||
SSEAStarNode RobotNode = GetClosesNode(startNode, goal, theta);
|
||||
var path = Find(RobotNode, goal, goalAngle, cancellationToken);
|
||||
if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) throw new OperationCanceledException();
|
||||
if (path is null || path.Count < 1) throw new Exception($"The path to {goal.Name} - {goal.Id} does not exist from [{startNode.Name} - {startNode.Id}]");
|
||||
path.Reverse();
|
||||
foreach (var node in path)
|
||||
{
|
||||
var nodedb = Nodes.FirstOrDefault(p => p.Id == node.Id) ?? throw new Exception($"Map data error: Node {node.Id} does not exist in the map");
|
||||
Path.Add(new GlobalNode
|
||||
{
|
||||
Id = nodedb.Id,
|
||||
X = nodedb.X,
|
||||
Y = nodedb.Y,
|
||||
Name = nodedb.Name,
|
||||
MapId = nodedb.MapId,
|
||||
Orientation = node.Orientation
|
||||
});
|
||||
}
|
||||
var startEdge = path.Count > 1 ? Edges.FirstOrDefault(e => e.StartNodeId == path[0].Id && e.EndNodeId == path[1].Id) : null;
|
||||
return ([.. Path], startEdge);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user