first commit -push

RobotNet.RobotManager/Services/Planner/ForkliftV2/ForkLiftPathPlannerV2.cs

@@ -0,0 +1,310 @@
+using RobotNet.MapShares;
+using RobotNet.MapShares.Dtos;
+using RobotNet.MapShares.Enums;
+using RobotNet.RobotManager.Services.Planner.AStar;
+using RobotNet.RobotManager.Services.Planner.Fokrlift;
+using RobotNet.RobotManager.Services.Planner.Space;
+using RobotNet.RobotShares.Enums;
+using RobotNet.RobotShares.Models;
+using RobotNet.Shares;
+
+namespace RobotNet.RobotManager.Services.Planner.ForkliftV2;
+
+public class ForkLiftPathPlannerV2 : IPathPlanner
+{
+    private List<NodeDto> Nodes = [];
+    private List<EdgeDto> Edges = [];
+    private const double ReverseDirectionAngleDegree = 80;
+    private const double RobotDirectionWithAngle = 90;
+    private const double Ratio = 0.1;
+
+    private readonly PathPlanningOptions Options = new()
+    {
+        LimitDistanceToEdge = 1,
+        LimitDistanceToNode = 0.3,
+        ResolutionSplit = 0.1,
+    };
+
+    public void SetData(NodeDto[] nodes, EdgeDto[] edges)
+    {
+        Nodes = [.. nodes];
+        Edges = [.. edges];
+    }
+
+    public void SetOptions(PathPlanningOptions options)
+    {
+        Options.LimitDistanceToNode = options.LimitDistanceToNode;
+        Options.LimitDistanceToEdge = options.LimitDistanceToEdge;
+        Options.ResolutionSplit = options.ResolutionSplit;
+    }
+
+    public static RobotDirection[] GetRobotDirectionInPath(RobotDirection currentDirection, List<NodeDto> nodeplanning, List<EdgeDto> edgePlanning)
+    {
+        RobotDirection[] RobotDirectionInNode = new RobotDirection[nodeplanning.Count];
+        if (nodeplanning.Count > 0) RobotDirectionInNode[0] = currentDirection;
+        if (nodeplanning.Count > 2)
+        {
+            for (int i = 1; i < nodeplanning.Count - 1; i++)
+            {
+                NodeDto startNode = MapEditorHelper.GetNearByNode(nodeplanning[i], nodeplanning[i - 1], edgePlanning[i - 1], Ratio);
+                NodeDto endNode = MapEditorHelper.GetNearByNode(nodeplanning[i], nodeplanning[i + 1], edgePlanning[i], Ratio); ;
+                var angle = MapEditorHelper.GetAngle(nodeplanning[i], startNode, endNode);
+                if (angle < ReverseDirectionAngleDegree) RobotDirectionInNode[i] = RobotDirectionInNode[i - 1] == RobotDirection.FORWARD ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
+                else RobotDirectionInNode[i] = RobotDirectionInNode[i - 1];
+            }
+        }
+        if (nodeplanning.Count > 1) RobotDirectionInNode[^1] = RobotDirectionInNode[^2];
+        return RobotDirectionInNode;
+    }
+
+    public static RobotDirection GetRobotDirection(NodeDto currentNode, NodeDto nearNode, EdgeDto edge, double robotInNodeAngle, bool isReverse)
+    {
+        NodeDto NearNode = MapEditorHelper.GetNearByNode(currentNode, nearNode, edge, Ratio);
+
+        var RobotNearNode = new NodeDto()
+        {
+            X = currentNode.X + Math.Cos(robotInNodeAngle * Math.PI / 180),
+            Y = currentNode.Y + Math.Sin(robotInNodeAngle * Math.PI / 180),
+        };
+        var angle = MapEditorHelper.GetAngle(currentNode, NearNode, RobotNearNode);
+        if (angle > ReverseDirectionAngleDegree) return isReverse ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
+        else return isReverse ? RobotDirection.FORWARD : RobotDirection.BACKWARD;
+    }
+
+    private static NodeDto[] CalculatorDirection(List<NodeDto> nodes, List<EdgeDto> edges, RobotDirection currentDirection)
+    {
+        var FinalDirection = GetRobotDirectionInPath(currentDirection, nodes, edges);
+        NodeDto[] returnNodes = [.. nodes];
+        foreach (var item in returnNodes)
+        {
+            item.Direction = MapCompute.GetNodeDirection(FinalDirection[nodes.IndexOf(item)]);
+        }
+        return returnNodes;
+    }
+
+    private List<EdgeDto> GetEdgesPlanning(List<NodeDto> nodes)
+    {
+        var EdgesPlanning = new List<EdgeDto>();
+        for (int i = 0; i < nodes.Count - 1; i++)
+        {
+            var edge = Edges.FirstOrDefault(e => e.StartNodeId == nodes[i].Id && e.EndNodeId == nodes[i + 1].Id ||
+                                                  e.EndNodeId == nodes[i].Id && e.StartNodeId == nodes[i + 1].Id);
+            if (edge is null)
+            {
+                if (i != 0) return [];
+                EdgesPlanning.Add(new EdgeDto()
+                {
+                    Id = Guid.NewGuid(),
+                    StartNodeId = nodes[i].Id,
+                    EndNodeId = nodes[i + 1].Id,
+                    DirectionAllowed = DirectionAllowed.Both,
+                    TrajectoryDegree = TrajectoryDegree.One,
+                });
+                continue;
+            }
+            bool isReverse = nodes[i].Id != edge.StartNodeId && edge.TrajectoryDegree == TrajectoryDegree.Three; ;
+            EdgesPlanning.Add(new()
+            {
+                Id = edge.Id,
+                StartNodeId = nodes[i].Id,
+                EndNodeId = nodes[i + 1].Id,
+                DirectionAllowed = edge.DirectionAllowed,
+                TrajectoryDegree = edge.TrajectoryDegree,
+                ControlPoint1X = isReverse ? edge.ControlPoint2X : edge.ControlPoint1X,
+                ControlPoint1Y = isReverse ? edge.ControlPoint2Y : edge.ControlPoint1Y,
+                ControlPoint2X = isReverse ? edge.ControlPoint1X : edge.ControlPoint2X,
+                ControlPoint2Y = isReverse ? edge.ControlPoint1Y : edge.ControlPoint2Y
+            });
+        }
+        return EdgesPlanning;
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanning(double x, double y, double theta, Guid goalId, CancellationToken? cancellationToken = null)
+    {
+        var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
+        if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
+
+        var AStarPathPlanner = new AStarPathPlanner(Nodes, Edges);
+        var Path = AStarPathPlanner.Planning(x,
+                                              y,
+                                              new NodeDto() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
+                                              Options.LimitDistanceToEdge,
+                                              Options.LimitDistanceToNode,
+                                              cancellationToken);
+        if (!Path.IsSuccess) return new(false, Path.Message);
+        if (Path.Data is null || Path.Data.Count < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{x}, {y}, {theta}]") { Data = ([goal], []) };
+        if (Path.Data.Count == 1) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
+
+        var edgeplannings = GetEdgesPlanning([.. Path.Data]);
+        return new(true) { Data = ([.. CalculatorDirection(Path.Data, edgeplannings, MapCompute.GetRobotDirection(Path.Data[0].Direction))], [.. edgeplannings]), };
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithAngle(double x, double y, double theta, Guid goalId, CancellationToken? cancellationToken = null)
+    {
+        var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
+        if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
+
+        var SSEAStarPathPlanner = new SSEAStarPathPlanner(Nodes, Edges);
+        var Path = SSEAStarPathPlanner.PlanningWithGoalAngle(x,
+                                              y,
+                                              theta,
+                                              new NodeDto() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y, Theta = goal.Theta },
+                                              Options.LimitDistanceToEdge,
+                                              Options.LimitDistanceToNode,
+                                              cancellationToken);
+        if (!Path.IsSuccess|| Path.Data is null || Path.Data.Count < 1)
+        {
+            var ForkliftV1 = new ForkliftPathPlanner();
+            ForkliftV1.SetData([.. Nodes], [.. Edges]);
+            return ForkliftV1.PathPlanningWithAngle(x,
+                                                    y,
+                                                    theta,
+                                                    goal.Id,
+                                                    cancellationToken);
+        }
+        else
+        {
+            if (Path.Data.Count == 1) return new(true, $"Robot đang đứng tại điểm đích [{goal.X}, {goal.Y}], robot: [{x}, {y}, {theta}]") { Data = ([goal], [])};
+
+            var edgeplannings = GetEdgesPlanning(Path.Data);
+            if (edgeplannings.Count < 1) return new(false, $"Không thể lấy ra các đoạn thuộc tuyến đường đã tìm ra");
+
+            return new(true) { Data = ([.. CalculatorDirection(Path.Data, edgeplannings, MapCompute.GetRobotDirection(Path.Data[0].Direction))], [.. edgeplannings]), };
+        }
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithStartDirection(double x, double y, double theta, Guid goalId, RobotDirection startDiretion = RobotDirection.NONE, CancellationToken? cancellationToken = null)
+    {
+        throw new NotImplementedException();
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithFinalDirection(double x, double y, double theta, Guid goalId, RobotDirection goalDirection = RobotDirection.NONE, CancellationToken? cancellationToken = null)
+    {
+        var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
+        if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
+
+        var SSEAStarPathPlanner = new SSEAStarPathPlanner(Nodes, Edges);
+        var Path = SSEAStarPathPlanner.PlanningWithFinalDirection(x,
+                                              y,
+                                              theta,
+                                              new NodeDto() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
+                                              goalDirection,
+                                              Options.LimitDistanceToEdge,
+                                              Options.LimitDistanceToNode,
+                                              cancellationToken);
+        if (!Path.IsSuccess || Path.Data is null || Path.Data.Count < 1)
+        {
+            var ForkliftV1 = new ForkliftPathPlanner();
+            ForkliftV1.SetData([.. Nodes], [.. Edges]);
+            return ForkliftV1.PathPlanningWithFinalDirection(x,
+                                                                  y,
+                                                                  theta,
+                                                                  goal.Id,
+                                                                  goalDirection,
+                                                                  cancellationToken);
+        }
+        else
+        {
+            if (Path.Data.Count == 1) return new(true, $"Robot đang đứng tại điểm đích [{goal.X}, {goal.Y}], robot: [{x}, {y}, {theta}]") { Data = ([goal], []) };
+
+            var edgeplannings = GetEdgesPlanning(Path.Data);
+            if (edgeplannings.Count < 1) return new(false, $"Không thể lấy ra các đoạn thuộc tuyến đường đã tìm ra");
+
+            return new(true) { Data = ([.. CalculatorDirection(Path.Data, edgeplannings, MapCompute.GetRobotDirection(Path.Data[0].Direction))], [.. edgeplannings]), };
+        }
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanning(Guid startNodeId, double theta, Guid goalId, CancellationToken? cancellationToken = null)
+    {
+        var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
+        if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
+
+        var startNode = Nodes.FirstOrDefault(n => n.Id == startNodeId);
+        if (startNode is null) return new(false, $"Điểm bắt đầu {startNodeId} không tồn tại trong map");
+
+        var AStarPathPlanner = new AStarPathPlanner(Nodes, Edges);
+        var Path = AStarPathPlanner.Planning(startNode,
+                                            goal,
+                                            cancellationToken);
+        if (!Path.IsSuccess) return new(false, Path.Message);
+        if (Path.Data is null || Path.Data.Length < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{startNode.Name} - {startNode.Id}]") { Data = ([goal], []) };
+        if (Path.Data.Length == 1) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
+
+        var edgeplannings = GetEdgesPlanning([.. Path.Data]);
+
+        return new(true) { Data = ([.. CalculatorDirection([..Path.Data], edgeplannings, MapCompute.GetRobotDirection(Path.Data[0].Direction))], [.. edgeplannings]), };
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithAngle(Guid startNodeId, double theta, Guid goalId, CancellationToken? cancellationToken = null)
+    {
+        var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
+        if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
+
+        var startNode = Nodes.FirstOrDefault(n => n.Id == startNodeId);
+        if (startNode is null) return new(false, $"Điểm bắt đầu {startNodeId} không tồn tại trong map");
+
+        var SSEAStarPathPlanner = new SSEAStarPathPlanner(Nodes, Edges);
+        var Path = SSEAStarPathPlanner.PlanningWithGoalAngle(startNode,
+                                                            theta,
+                                                            goal,
+                                                            cancellationToken);
+        if (!Path.IsSuccess || Path.Data is null || Path.Data.Length < 1)
+        {
+            var ForkliftV1 = new ForkliftPathPlanner();
+            ForkliftV1.SetData([.. Nodes], [.. Edges]);
+            return ForkliftV1.PathPlanningWithAngle(startNodeId,
+                                                    theta,
+                                                    goal.Id,
+                                                    cancellationToken);
+        }
+        else
+        {
+            if (Path.Data.Length == 1) return new(true, $"Robot đang đứng tại điểm đích [{goal.X}, {goal.Y}], robot: [{startNode.Name} - {startNode.Id}]") { Data = ([goal], []) };
+
+            var edgeplannings = GetEdgesPlanning([.. Path.Data]);
+            if (edgeplannings.Count < 1) return new(false, $"Không thể lấy ra các đoạn thuộc tuyến đường đã tìm ra");
+
+            return new(true) { Data = ([.. CalculatorDirection([.. Path.Data], edgeplannings, MapCompute.GetRobotDirection(Path.Data[0].Direction))], [.. edgeplannings]), };
+        }
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithStartDirection(Guid startNodeId, double theta, Guid goalId, RobotDirection startDiretion = RobotDirection.NONE, CancellationToken? cancellationToken = null)
+    {
+        throw new NotImplementedException();
+    }
+
+    public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithFinalDirection(Guid startNodeId, double theta, Guid goalId, RobotDirection goalDirection = RobotDirection.NONE, CancellationToken? cancellationToken = null)
+    {
+        var goal = Nodes.FirstOrDefault(n => n.Id == goalId);
+        if (goal is null) return new(false, $"Đích đến {goalId} không tồn tại trong map");
+
+        var startNode = Nodes.FirstOrDefault(n => n.Id == startNodeId);
+        if (startNode is null) return new(false, $"Điểm bắt đầu {startNodeId} không tồn tại trong map");
+
+        var SSEAStarPathPlanner = new SSEAStarPathPlanner(Nodes, Edges);
+        var Path = SSEAStarPathPlanner.PlanningWithFinalDirection(startNode,
+                                                                  theta,
+                                                                  goal,
+                                                                  goalDirection,
+                                                                  cancellationToken);
+        if (!Path.IsSuccess || Path.Data is null || Path.Data.Length < 1)
+        {
+            var ForkliftV1 = new ForkliftPathPlanner();
+            ForkliftV1.SetData([.. Nodes], [.. Edges]);
+            return ForkliftV1.PathPlanningWithFinalDirection(startNodeId,
+                                                                theta,
+                                                                goal.Id,
+                                                                goalDirection,
+                                                                cancellationToken);
+        }
+        else
+        {
+            if (Path.Data.Length == 1) return new(true, $"Robot đang đứng tại điểm đích [{goal.X}, {goal.Y}], robot: [{startNode.Name} - {startNode.Id}]") { Data = ([goal], []) };
+
+            var edgeplannings = GetEdgesPlanning([.. Path.Data]);
+            if (edgeplannings.Count < 1) return new(false, $"Không thể lấy ra các đoạn thuộc tuyến đường đã tìm ra");
+
+            return new(true) { Data = ([.. CalculatorDirection([.. Path.Data], edgeplannings, MapCompute.GetRobotDirection(Path.Data[0].Direction))], [.. edgeplannings]), };
+        }
+    }
+}

RobotNet.RobotManager/Services/Planner/ForkliftV2/SSEAStarNode.cs

@@ -0,0 +1,31 @@
+using RobotNet.RobotShares.Enums;
+
+namespace RobotNet.RobotManager.Services.Planner.ForkliftV2;
+
+#nullable disable
+
+public class SSEAStarNode
+{
+    public Guid Id { get; set; }
+    public double X { get; set; }
+    public double Y { get; set; }
+    public RobotDirection Direction { get; set; }
+    public double Cost { get; set; }
+    public double Heuristic { get; set; }
+    public double TotalCost => Cost + Heuristic;
+    public string Name { get; set; }
+    public SSEAStarNode Parent { get; set; }
+    public List<SSEAStarNode> NegativeNodes { get; set; } = [];
+
+    public override bool Equals(object obj)
+    {
+        if (obj is SSEAStarNode other)
+            return Id == other.Id && Direction == other.Direction;
+        return false;
+    }
+
+    public override int GetHashCode()
+    {
+        return HashCode.Combine(Id, Direction);
+    }
+}

RobotNet.RobotManager/Services/Planner/ForkliftV2/SSEAStarPathPlanner.cs

@@ -0,0 +1,621 @@
+using RobotNet.MapShares;
+using RobotNet.MapShares.Dtos;
+using RobotNet.MapShares.Enums;
+using RobotNet.RobotManager.Services.Planner.Space;
+using RobotNet.RobotShares.Enums;
+using RobotNet.Shares;
+
+namespace RobotNet.RobotManager.Services.Planner.ForkliftV2;
+
+public class SSEAStarPathPlanner(List<NodeDto> Nodes, List<EdgeDto> Edges)
+{
+    private NodeDto? 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);
+    }
+
+    //public EdgeDto? GetClosesEdge(double x, double y, double limitDistance, CancellationToken? cancellationToken = null)
+    //{
+    //    if (cancellationToken?.IsCancellationRequested == true) return null;
+    //    RTree RTree = new(Nodes, Edges);
+    //    return RTree.FindNearest(x, y, limitDistance);
+    //}
+
+    public EdgeDto? GetClosesEdge(double x, double y, double limitDistance, CancellationToken? cancellationToken = null)
+    {
+        double minDistance = double.MaxValue;
+        EdgeDto? edgeResult = null;
+        foreach (var edge in Edges)
+        {
+            if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) return null;
+            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 getDistance = MapCompute.DistanceToEdge(x, y, edge, startNode, endNode);
+            if (getDistance.IsSuccess)
+            {
+                if (getDistance.Data < minDistance)
+                {
+                    minDistance = getDistance.Data;
+                    edgeResult = edge;
+                }
+            }
+        }
+        if (minDistance <= limitDistance) return edgeResult;
+        else return null;
+    }
+
+    public List<NodeDto> GetNegativeNode(Guid nodeId, CancellationToken? cancellationToken = null)
+    {
+        var node = Nodes.FirstOrDefault(p => p.Id == nodeId);
+        if (node is null) return [];
+
+        var listNodesNegative = new List<NodeDto>();
+        var listPaths = Edges.Where(p => p.EndNodeId == nodeId || p.StartNodeId == nodeId);
+        foreach (var path in listPaths)
+        {
+            if (cancellationToken.HasValue && cancellationToken.Value.IsCancellationRequested) return [];
+            if (path.StartNodeId == node.Id && (path.DirectionAllowed == DirectionAllowed.Both || path.DirectionAllowed == DirectionAllowed.Forward))
+            {
+                var nodeAdd = Nodes.FirstOrDefault(p => p.Id == path.EndNodeId);
+                if (nodeAdd != null) listNodesNegative.Add(nodeAdd);
+                continue;
+            }
+            if (path.EndNodeId == node.Id && (path.DirectionAllowed == DirectionAllowed.Both || path.DirectionAllowed == DirectionAllowed.Backward))
+            {
+                var nodeAdd = Nodes.FirstOrDefault(p => p.Id == path.StartNodeId);
+                if (nodeAdd != null) listNodesNegative.Add(nodeAdd);
+                continue;
+            }
+        }
+        return listNodesNegative;
+    }
+
+    private double GetNegativeCost(SSEAStarNode currentNode, SSEAStarNode negativeNode)
+    {
+        var negativeEdges = Edges.Where(e => e.StartNodeId == currentNode.Id && e.EndNodeId == negativeNode.Id ||
+                                            e.StartNodeId == negativeNode.Id && e.EndNodeId == currentNode.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 = MapEditorHelper.GetEdgeLength(new()
+            {
+                X1 = startNode.X,
+                Y1 = startNode.Y,
+                X2 = endNode.X,
+                Y2 = endNode.Y,
+                ControlPoint1X = edge.ControlPoint1X,
+                ControlPoint1Y = edge.ControlPoint1Y,
+                ControlPoint2X = edge.ControlPoint2X,
+                ControlPoint2Y = edge.ControlPoint2Y,
+                TrajectoryDegree = edge.TrajectoryDegree,
+            });
+            if (distance < minDistance) minDistance = distance;
+        }
+        return minDistance != double.MaxValue ? minDistance : 0;
+    }
+
+    private List<SSEAStarNode> GetNegativeAStarNode(SSEAStarNode nodeCurrent, NodeDto endNode, CancellationToken? cancellationToken = null)
+    {
+        var possiblePointNegative = new List<SSEAStarNode>();
+        if (nodeCurrent.Id == endNode.Id) return possiblePointNegative;
+
+        var listNodesNegative = GetNegativeNode(nodeCurrent.Id, cancellationToken);
+
+        foreach (var item in listNodesNegative)
+        {
+            if (cancellationToken.HasValue && cancellationToken.Value.IsCancellationRequested) return [];
+            if (nodeCurrent.Parent is null) continue;
+            var nodeDtoCurrent = Nodes.FirstOrDefault(n => n.Id == nodeCurrent.Id);
+            var nodeDtoNegative = Nodes.FirstOrDefault(n => n.Id == item.Id);
+            var nodeDtoParent = Nodes.FirstOrDefault(n => n.Id == nodeCurrent.Parent.Id);
+            var negativeEdge = Edges.FirstOrDefault(e => e.StartNodeId == nodeCurrent.Id && e.EndNodeId == item.Id || e.EndNodeId == nodeCurrent.Id && e.StartNodeId == item.Id);
+            var parentEdge = Edges.FirstOrDefault(e => e.StartNodeId == nodeCurrent.Id && e.EndNodeId == nodeCurrent.Parent.Id || e.EndNodeId == nodeCurrent.Id && e.StartNodeId == nodeCurrent.Parent.Id);
+
+            if (nodeDtoCurrent is null || nodeDtoNegative is null || negativeEdge is null) continue;
+
+            var nearNodeNevgative = MapEditorHelper.GetNearByNode(nodeDtoCurrent, nodeDtoNegative, negativeEdge, 0.01);
+            var nearNodeParent = nodeDtoParent is not null && parentEdge is not null ? MapEditorHelper.GetNearByNode(nodeDtoCurrent, nodeDtoParent, parentEdge, 0.01) :
+                                 new()
+                                 {
+                                     Id = nodeCurrent.Parent.Id,
+                                     X = nodeCurrent.Parent.X,
+                                     Y = nodeCurrent.Parent.Y,
+                                     Name = nodeCurrent.Parent.Name
+                                 };
+
+            var angle = MapEditorHelper.GetAngle(nodeDtoCurrent, nearNodeNevgative, nearNodeParent);
+            RobotDirection direction = angle < 89 ? nodeCurrent.Direction == RobotDirection.FORWARD ? RobotDirection.BACKWARD : RobotDirection.FORWARD : nodeCurrent.Direction;
+
+            var nodeNegative = new SSEAStarNode
+            {
+                Id = item.Id,
+                X = item.X,
+                Y = item.Y,
+                Name = item.Name,
+                Direction = direction,
+                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 + (direction == RobotDirection.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 + (direction == RobotDirection.BACKWARD ? distance : 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, NodeDto endNode, RobotDirection 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 == endNode.Id)
+                {
+                    if (checkNode.Direction == goalDirection)
+                    {
+                        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 = GetNegativeAStarNode(checkNode, endNode, cancellationToken);
+                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.Direction == node.Direction);
+                    if (activeNode is not null && activeNode.TotalCost > node.TotalCost)
+                    {
+                        activeNodes.Items.Remove(activeNode);
+                        activeNodes.Enqueue(node);
+                    }
+                    else
+                    {
+                        activeNodes.Enqueue(node);
+                    }
+                }
+            }
+
+            return [];
+        }
+        catch
+        {
+            return [];
+        }
+    }
+
+    public List<SSEAStarNode> Find(SSEAStarNode startNode, NodeDto endNode, 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 == endNode.Id)
+                {
+                    if (checkNode.Parent is not null)
+                    {
+                        var nodeParentDto = Nodes.FirstOrDefault(n => n.Id == checkNode.Parent.Id);
+                        var edge = Edges.FirstOrDefault(e => e.StartNodeId == checkNode.Id && e.EndNodeId == checkNode.Parent.Id || e.EndNodeId == checkNode.Id && e.StartNodeId == checkNode.Parent.Id);
+                        if (edge is not null && nodeParentDto is not null)
+                        {
+                            var nearParent = MapEditorHelper.GetNearByNode(endNode, nodeParentDto, edge, 0.01);
+                            var nearGoalNode = new NodeDto()
+                            {
+                                X = endNode.X + Math.Cos(endNode.Theta * Math.PI / 180),
+                                Y = endNode.Y + Math.Sin(endNode.Theta * Math.PI / 180),
+                            };
+
+                            var angle = MapEditorHelper.GetAngle(endNode, nearParent, nearGoalNode);
+                            RobotDirection goalDirection = angle < 89 ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
+                            if (checkNode.Direction == 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 = GetNegativeAStarNode(checkNode, endNode, cancellationToken);
+                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.Direction == node.Direction);
+                    if (activeNode is not null && activeNode.TotalCost > node.TotalCost)
+                    {
+                        activeNodes.Items.Remove(activeNode);
+                        activeNodes.Enqueue(node);
+                    }
+                    else
+                    {
+                        activeNodes.Enqueue(node);
+                    }
+                }
+            }
+
+            return [];
+        }
+        catch
+        {
+            return [];
+        }
+    }
+
+    private SSEAStarNode GetClosesNode(NodeDto closesNode, NodeDto goal, double theta, CancellationToken? cancellationToken = null)
+    {
+        SSEAStarNode closesAStarNode = new()
+        {
+            Id = closesNode.Id,
+            X = closesNode.X,
+            Y = closesNode.Y,
+            Name = closesNode.Name,
+        };
+        foreach (var negativeNode in GetNegativeNode(closesAStarNode.Id, cancellationToken))
+        {
+            SSEAStarNode closesAStarNodeParent = new()
+            {
+                Id = closesNode.Id,
+                X = closesNode.X,
+                Y = closesNode.Y,
+                Name = closesNode.Name,
+            };
+            var RobotNearNode = new NodeDto()
+            {
+                X = closesAStarNode.X + Math.Cos(theta * Math.PI / 180),
+                Y = closesAStarNode.Y + Math.Sin(theta * Math.PI / 180),
+            };
+
+            var angle = MapEditorHelper.GetAngle(closesNode, negativeNode, RobotNearNode);
+            RobotDirection direction = angle < 91 ? RobotDirection.FORWARD : RobotDirection.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 += direction == RobotDirection.BACKWARD ? cost * Math.Sqrt(2) / 2 : 0.0;
+            closesAStarNodeParent.Direction = direction;
+            closesAStarNode.NegativeNodes.Add(new()
+            {
+                Id = negativeNode.Id,
+                X = negativeNode.X,
+                Y = negativeNode.Y,
+                Name = negativeNode.Name,
+                Direction = direction,
+                Cost = cost,
+                Heuristic = Math.Abs(goal.X - negativeNode.X) + Math.Abs(goal.Y - negativeNode.Y),
+                Parent = closesAStarNodeParent,
+            });
+        }
+        return closesAStarNode;
+    }
+
+    private static SSEAStarNode[] GetClosesEdge(EdgeDto closesEdge, NodeDto goal, NodeDto startNodeForward, NodeDto startNodeBackward, SSEAStarNode robotNode, double theta)
+    {
+        List<SSEAStarNode> negativeNodes = [];
+        if (closesEdge.DirectionAllowed == DirectionAllowed.Both || closesEdge.DirectionAllowed == DirectionAllowed.Backward)
+        {
+            SSEAStarNode closesAStarNodeParent = new()
+            {
+                Id = robotNode.Id,
+                X = robotNode.X,
+                Y = robotNode.Y,
+                Name = robotNode.Name,
+            };
+            var RobotNearNode = new NodeDto()
+            {
+                X = robotNode.X + Math.Cos(theta * Math.PI / 180),
+                Y = robotNode.Y + Math.Sin(theta * Math.PI / 180),
+            };
+            var angle = MapEditorHelper.GetAngle(new() { X = robotNode.X, Y = robotNode.Y, Theta = theta }, startNodeForward, RobotNearNode);
+            RobotDirection direction = angle < 91 ? RobotDirection.FORWARD : RobotDirection.BACKWARD;
+
+            double cost = Math.Sqrt(Math.Pow(robotNode.X - startNodeBackward.X, 2) + Math.Pow(robotNode.Y - startNodeBackward.Y, 2));
+            cost += direction == RobotDirection.BACKWARD ? cost * Math.Sqrt(2) / 2 : 0.0;
+            closesAStarNodeParent.Direction = direction;
+            negativeNodes.Add(new()
+            {
+                Id = startNodeForward.Id,
+                X = startNodeForward.X,
+                Y = startNodeForward.Y,
+                Name = startNodeForward.Name,
+                Direction = direction,
+                Cost = cost,
+                Heuristic = Math.Abs(goal.X - startNodeForward.X) + Math.Abs(goal.Y - startNodeForward.Y),
+                Parent = closesAStarNodeParent,
+            });
+        }
+        if (closesEdge.DirectionAllowed == DirectionAllowed.Both || closesEdge.DirectionAllowed == DirectionAllowed.Forward)
+        {
+            SSEAStarNode closesAStarNodeParent = new()
+            {
+                Id = robotNode.Id,
+                X = robotNode.X,
+                Y = robotNode.Y,
+                Name = robotNode.Name,
+            };
+            var RobotNearNode = new NodeDto()
+            {
+                X = robotNode.X + Math.Cos(theta * Math.PI / 180),
+                Y = robotNode.Y + Math.Sin(theta * Math.PI / 180),
+            };
+            var angle = MapEditorHelper.GetAngle(new() { X = robotNode.X, Y = robotNode.Y, Theta = theta }, startNodeBackward, RobotNearNode);
+            RobotDirection direction = angle < 91 ? RobotDirection.FORWARD : RobotDirection.BACKWARD;
+
+            double cost = Math.Sqrt(Math.Pow(robotNode.X - startNodeBackward.X, 2) + Math.Pow(robotNode.Y - startNodeBackward.Y, 2));
+            cost += direction == RobotDirection.BACKWARD ? cost * Math.Sqrt(2) / 2 : 0.0;
+            closesAStarNodeParent.Direction = direction;
+            negativeNodes.Add(new()
+            {
+                Id = startNodeBackward.Id,
+                X = startNodeBackward.X,
+                Y = startNodeBackward.Y,
+                Name = startNodeBackward.Name,
+                Direction = direction,
+                Cost = cost,
+                Heuristic = Math.Abs(goal.X - startNodeBackward.X) + Math.Abs(goal.Y - startNodeBackward.Y),
+                Parent = closesAStarNodeParent,
+            });
+        }
+        return [.. negativeNodes];
+    }
+
+    public MessageResult<List<NodeDto>> PlanningWithFinalDirection(double x, double y, double theta, NodeDto goal, RobotDirection goalDirection, double maxDistanceToEdge, double maxDistanceToNode, CancellationToken? cancellationToken = null)
+    {
+        var Path = new List<NodeDto>();
+
+        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 new(true, "Robot đang đứng tại điểm đích") { Data = [goal] };
+            RobotNode = GetClosesNode(closesNode, goal, theta);
+        }
+        else
+        {
+            var closesEdge = GetClosesEdge(x, y, maxDistanceToEdge, cancellationToken);
+            if (closesEdge is null)
+            {
+                return new(false, "Robot đang quá xa tuyến đường");
+            }
+
+            var startNodeForward = Nodes.FirstOrDefault(p => p.Id == closesEdge.StartNodeId);
+            var startNodeBackward = Nodes.FirstOrDefault(p => p.Id == closesEdge.EndNodeId);
+            if (startNodeForward is null || startNodeBackward is null)
+            {
+                return new(false, "Dữ liệu lỗi: điểm chặn của edge gần nhất không tồn tại");
+            }
+            if (startNodeForward.Id == goal.Id && (closesEdge.DirectionAllowed == DirectionAllowed.Both || closesEdge.DirectionAllowed == DirectionAllowed.Backward))
+            {
+                Path.Add(startNodeBackward);
+                Path.Add(startNodeForward);
+                return new(true) { Data = Path };
+            }
+            if (startNodeBackward.Id == goal.Id && (closesEdge.DirectionAllowed == DirectionAllowed.Both || closesEdge.DirectionAllowed == DirectionAllowed.Forward))
+            {
+                Path.Add(startNodeForward);
+                Path.Add(startNodeBackward);
+                return new(true) { Data = Path };
+            }
+            RobotNode.NegativeNodes.AddRange(GetClosesEdge(closesEdge, goal, startNodeForward, startNodeBackward, RobotNode, theta));
+        }
+
+        if (RobotNode.NegativeNodes.Count < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{x}, {y}]");
+
+        var path = Find(RobotNode, goal, goalDirection, cancellationToken);
+        if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) return new(false, "Yêu cầu hủy bỏ");
+        if (path is null || path.Count < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{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,
+                    Direction = MapCompute.GetNodeDirection(node.Direction),
+                });
+                continue;
+            }
+            var nodedb = Nodes.FirstOrDefault(p => p.Id == node.Id);
+            if (nodedb is null) return new(false, "Dữ liệu bản đồ có lỗi");
+            nodedb.Direction = MapCompute.GetNodeDirection(node.Direction);
+            Path.Add(nodedb);
+        }
+        return new(true) { Data = Path };
+    }
+
+    public MessageResult<List<NodeDto>> PlanningWithGoalAngle(double x, double y, double theta, NodeDto goal, double maxDistanceToEdge, double maxDistanceToNode, CancellationToken? cancellationToken = null)
+    {
+        var Path = new List<NodeDto>();
+
+        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 new(true, "Robot đang đứng tại điểm đích") { Data = [goal] };
+            RobotNode = GetClosesNode(closesNode, goal, theta);
+        }
+        else
+        {
+            var closesEdge = GetClosesEdge(x, y, maxDistanceToEdge, cancellationToken);
+            if (closesEdge is null)
+            {
+                return new(false, "Robot đang quá xa tuyến đường");
+            }
+
+            var startNodeForward = Nodes.FirstOrDefault(p => p.Id == closesEdge.StartNodeId);
+            var startNodeBackward = Nodes.FirstOrDefault(p => p.Id == closesEdge.EndNodeId);
+            if (startNodeForward is null || startNodeBackward is null)
+            {
+                return new(false, "Dữ liệu lỗi: điểm chặn của edge gần nhất không tồn tại");
+            }
+            if (startNodeForward.Id == goal.Id && (closesEdge.DirectionAllowed == DirectionAllowed.Both || closesEdge.DirectionAllowed == DirectionAllowed.Backward))
+            {
+                Path.Add(startNodeBackward);
+                Path.Add(startNodeForward);
+                return new(true) { Data = Path };
+            }
+            if (startNodeBackward.Id == goal.Id && (closesEdge.DirectionAllowed == DirectionAllowed.Both || closesEdge.DirectionAllowed == DirectionAllowed.Forward))
+            {
+                Path.Add(startNodeForward);
+                Path.Add(startNodeBackward);
+                return new(true) { Data = Path };
+            }
+            RobotNode.NegativeNodes.AddRange(GetClosesEdge(closesEdge, goal, startNodeForward, startNodeBackward, RobotNode, theta));
+        }
+
+        if (RobotNode.NegativeNodes.Count < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{x}, {y}]");
+
+        var path = Find(RobotNode, goal, cancellationToken);
+        if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) return new(false, "Yêu cầu hủy bỏ");
+        if (path is null || path.Count < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{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,
+                    Direction = MapCompute.GetNodeDirection(node.Direction),
+                });
+                continue;
+            }
+            var nodedb = Nodes.FirstOrDefault(p => p.Id == node.Id);
+            if (nodedb is null) return new(false, "Dữ liệu bản đồ có lỗi");
+            nodedb.Direction = MapCompute.GetNodeDirection(node.Direction);
+            Path.Add(nodedb);
+        }
+        return new(true) { Data = Path };
+    }
+
+    public MessageResult<NodeDto[]> PlanningWithFinalDirection(NodeDto startNode, double theta, NodeDto goal, RobotDirection goalDirection, CancellationToken? cancellationToken = null)
+    {
+        var Path = new List<NodeDto>();
+        SSEAStarNode RobotNode = GetClosesNode(startNode, goal, theta);
+        var path = Find(RobotNode, goal, goalDirection, cancellationToken);
+        if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) return new(false, "Yêu cầu hủy bỏ");
+        if (path is null || path.Count < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{startNode.Name} - {startNode.Id}]");
+        path.Reverse();
+        foreach (var node in path)
+        {
+            var nodedb = Nodes.FirstOrDefault(p => p.Id == node.Id);
+            if (nodedb is null) return new(false, "Dữ liệu bản đồ có lỗi");
+            nodedb.Direction = MapCompute.GetNodeDirection(node.Direction);
+            Path.Add(nodedb);
+        }
+        return new(true) { Data = [.. Path] };
+    }
+
+    public MessageResult<NodeDto[]> PlanningWithGoalAngle(NodeDto startNode, double theta, NodeDto goal, CancellationToken? cancellationToken = null)
+    {
+        var Path = new List<NodeDto>();
+        SSEAStarNode RobotNode = GetClosesNode(startNode, goal, theta);
+        var path = Find(RobotNode, goal, cancellationToken);
+        if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) return new(false, "Yêu cầu hủy bỏ");
+        if (path is null || path.Count < 1) return new(false, $"Đường đi đến {goal.Name} - {goal.Id} không tồn tại từ [{startNode.Name} - {startNode.Id}]");
+        path.Reverse();
+        foreach (var node in path)
+        {
+            var nodedb = Nodes.FirstOrDefault(p => p.Id == node.Id);
+            if (nodedb is null) return new(false, "Dữ liệu bản đồ có lỗi");
+            nodedb.Direction = MapCompute.GetNodeDirection(node.Direction);
+            Path.Add(nodedb);
+        }
+        return new(true) { Data = [.. Path] };
+    }
+}