Initial commit

2026-07-03 16:31:37 +07:00
commit 899c7c637d
1939 changed files with 641750 additions and 0 deletions
--- a/srcs/RobotNet10/Commons/RobotNet10.GlobalPathPlanner/ForkliftV2/ForkLiftPathPlannerV2.cs
+++ b/srcs/RobotNet10/Commons/RobotNet10.GlobalPathPlanner/ForkliftV2/ForkLiftPathPlannerV2.cs
@@ -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();
+        }
+    }
+}