332 lines
17 KiB
C#
332 lines
17 KiB
C#
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();
|
|
}
|
|
}
|
|
}
|