311 lines
17 KiB
C#
311 lines
17 KiB
C#
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]), };
|
|
}
|
|
}
|
|
}
|