352 lines
15 KiB
C#
352 lines
15 KiB
C#
using RobotNet.MapShares;
|
|
using RobotNet.MapShares.Dtos;
|
|
using RobotNet.MapShares.Enums;
|
|
using RobotNet.RobotManager.Services.Planner;
|
|
using RobotNet.RobotManager.Services.Planner.Space;
|
|
using RobotNet.RobotShares.Enums;
|
|
using RobotNet.Shares;
|
|
using System.Collections.Generic;
|
|
|
|
namespace RobotNet.RobotManager.Services;
|
|
|
|
public enum PathPlanningType
|
|
{
|
|
None,
|
|
Angle,
|
|
StartDirection,
|
|
EndDirection,
|
|
}
|
|
|
|
public class PathPlanner(IConfiguration Configuration, MapManager MapManager)
|
|
{
|
|
private readonly double ResolutionSplit = Configuration.GetValue("PathPlanning:ResolutionSplit", 0.1);
|
|
private readonly PathPlanningType PathPlanningType = Configuration.GetValue("PathPlanning:Type", PathPlanningType.None);
|
|
private readonly RobotDirection StartDirection = Configuration.GetValue("PathPlanning:StartDirection", RobotDirection.NONE);
|
|
private readonly RobotDirection EndDirection = Configuration.GetValue("PathPlanning:EndDirection", RobotDirection.NONE);
|
|
|
|
public async Task<MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)>> Planning(double xRef, double yRef, double thetaRef, NavigationType type, Guid mapId, string nodeName)
|
|
{
|
|
var map = await MapManager.GetMapData(mapId);
|
|
if (!map.IsSuccess) return new(false, map.Message);
|
|
if (map.Data is null) return new(false, "Dữ liệu bản đồ có lỗi");
|
|
|
|
var goalNode = map.Data.Nodes.FirstOrDefault(n => n.Name == nodeName);
|
|
if (goalNode is null) return new(false, "Đích đến không tồn tại");
|
|
|
|
PathPlannerManager PathPlannerManager = new();
|
|
var PathPlanner = PathPlannerManager.GetPathPlanningService(type);
|
|
PathPlanner.SetData(map.Data.Nodes, map.Data.Edges);
|
|
|
|
if (type == NavigationType.Forklift) return PathPlanner.PathPlanningWithAngle(xRef, yRef, thetaRef, goalNode.Id);
|
|
else
|
|
{
|
|
return PathPlanningType switch
|
|
{
|
|
PathPlanningType.None => PathPlanner.PathPlanning(xRef, yRef, thetaRef, goalNode.Id),
|
|
PathPlanningType.Angle => PathPlanner.PathPlanningWithAngle(xRef, yRef, thetaRef, goalNode.Id),
|
|
PathPlanningType.StartDirection => PathPlanner.PathPlanningWithStartDirection(xRef, yRef, thetaRef, goalNode.Id, StartDirection),
|
|
PathPlanningType.EndDirection => PathPlanner.PathPlanningWithFinalDirection(xRef, yRef, thetaRef, goalNode.Id, EndDirection),
|
|
_ => PathPlanner.PathPlanning(xRef, yRef, thetaRef, goalNode.Id),
|
|
};
|
|
}
|
|
}
|
|
|
|
public async Task<MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)>> Planning(Guid startNodeId, double thetaRef, NavigationType type, Guid mapId, Guid goalId)
|
|
{
|
|
var map = await MapManager.GetMapData(mapId);
|
|
if (!map.IsSuccess) return new(false, map.Message);
|
|
if (map.Data is null) return new(false, "Dữ liệu bản đồ có lỗi");
|
|
|
|
PathPlannerManager PathPlannerManager = new();
|
|
var PathPlanner = PathPlannerManager.GetPathPlanningService(type);
|
|
PathPlanner.SetData(map.Data.Nodes, map.Data.Edges);
|
|
|
|
if (type == NavigationType.Forklift) return PathPlanner.PathPlanningWithAngle(startNodeId, thetaRef, goalId);
|
|
else
|
|
{
|
|
return PathPlanningType switch
|
|
{
|
|
PathPlanningType.None => PathPlanner.PathPlanning(startNodeId, thetaRef, goalId),
|
|
PathPlanningType.Angle => PathPlanner.PathPlanningWithAngle(startNodeId, thetaRef, goalId),
|
|
PathPlanningType.StartDirection => PathPlanner.PathPlanningWithStartDirection(startNodeId, thetaRef, goalId, StartDirection),
|
|
PathPlanningType.EndDirection => PathPlanner.PathPlanningWithFinalDirection(startNodeId, thetaRef, goalId, EndDirection),
|
|
_ => PathPlanner.PathPlanning(startNodeId, thetaRef, goalId),
|
|
};
|
|
}
|
|
}
|
|
|
|
public static MessageResult<NodeDto[]> PathSplit(NodeDto[] nodes, EdgeDto[] edges, double resolutionSplit)
|
|
{
|
|
if (nodes.Length < 1 || edges.Length == 0) return new(false, "Dữ liệu không hợp lệ");
|
|
List<NodeDto> pathSplit = [];
|
|
pathSplit.Add(new()
|
|
{
|
|
Id = nodes[0].Id,
|
|
X = nodes[0].X,
|
|
Y = nodes[0].Y,
|
|
Theta = nodes[0].Theta,
|
|
Direction = nodes[0].Direction,
|
|
Name = nodes[0].Name,
|
|
Actions = "CheckingNode"
|
|
});
|
|
foreach (var edge in edges)
|
|
{
|
|
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 new(false, "Dữ liệu lỗi: Điểm đầu cuối của edge không có trong danh sách nodes");
|
|
NodeDto controlNode = new();
|
|
|
|
var EdgeCaculatorModel = new EdgeCaculatorModel()
|
|
{
|
|
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,
|
|
};
|
|
var length = MapEditorHelper.GetEdgeLength(EdgeCaculatorModel);
|
|
|
|
if (length <= 0) continue;
|
|
double step = resolutionSplit / length;
|
|
|
|
for (double t = step; t <= 1 - step; t += step)
|
|
{
|
|
(double x, double y) = MapEditorHelper.Curve(t, EdgeCaculatorModel);
|
|
pathSplit.Add(new()
|
|
{
|
|
Id = Guid.NewGuid(),
|
|
X = x,
|
|
Y = y,
|
|
Theta = startNode.Theta,
|
|
Direction = startNode.Direction,
|
|
});
|
|
}
|
|
|
|
pathSplit.Add(new()
|
|
{
|
|
Id = endNode.Id,
|
|
X = endNode.X,
|
|
Y = endNode.Y,
|
|
Theta = endNode.Theta,
|
|
Direction = endNode.Direction,
|
|
Name = endNode.Name,
|
|
Actions = "CheckingNode"
|
|
});
|
|
}
|
|
return new(true) { Data = [.. pathSplit] };
|
|
}
|
|
|
|
public MessageResult<NodeDto[]> PathSplit(NodeDto[] nodes, EdgeDto[] edges)
|
|
{
|
|
if (nodes.Length < 1 || edges.Length == 0) return new(false, "Dữ liệu không hợp lệ");
|
|
List<NodeDto> pathSplit = [];
|
|
pathSplit.Add(new()
|
|
{
|
|
Id = nodes[0].Id,
|
|
X = nodes[0].X,
|
|
Y = nodes[0].Y,
|
|
Theta = nodes[0].Theta,
|
|
Direction = nodes[0].Direction,
|
|
Name = nodes[0].Name,
|
|
Actions = "CheckingNode"
|
|
});
|
|
foreach (var edge in edges)
|
|
{
|
|
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 new(false, "Dữ liệu lỗi: Điểm đầu cuối của edge không có trong danh sách nodes");
|
|
NodeDto controlNode = new();
|
|
|
|
var EdgeCaculatorModel = new EdgeCaculatorModel()
|
|
{
|
|
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,
|
|
};
|
|
var length = MapEditorHelper.GetEdgeLength(EdgeCaculatorModel);
|
|
|
|
if (length <= 0) continue;
|
|
double step = ResolutionSplit / length;
|
|
|
|
for (double t = step; t <= 1 - step; t += step)
|
|
{
|
|
(double x, double y) = MapEditorHelper.Curve(t, EdgeCaculatorModel);
|
|
pathSplit.Add(new()
|
|
{
|
|
Id = Guid.NewGuid(),
|
|
X = x,
|
|
Y = y,
|
|
Theta = startNode.Theta,
|
|
Direction = startNode.Direction,
|
|
});
|
|
}
|
|
|
|
pathSplit.Add(new()
|
|
{
|
|
Id = endNode.Id,
|
|
X = endNode.X,
|
|
Y = endNode.Y,
|
|
Theta = endNode.Theta,
|
|
Direction = endNode.Direction,
|
|
Name = endNode.Name,
|
|
Actions = "CheckingNode"
|
|
});
|
|
}
|
|
return new(true) { Data = [.. pathSplit] };
|
|
}
|
|
|
|
public static RobotDirection GetRobotStartDirection(NodeDto currentNode, NodeDto nearNode, EdgeDto edge, double robotInNodeAngle)
|
|
{
|
|
NodeDto NearNode = MapEditorHelper.GetNearByNode(currentNode, nearNode, edge, 0.1);
|
|
|
|
var RobotNearNode = new NodeDto()
|
|
{
|
|
X = currentNode.X + Math.Cos(robotInNodeAngle * Math.PI / 180),
|
|
Y = currentNode.Y + Math.Sin(robotInNodeAngle * Math.PI / 180),
|
|
};
|
|
return MapEditorHelper.GetAngle(currentNode, NearNode, RobotNearNode) > 89 ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
|
|
}
|
|
|
|
public static RobotDirection[] GetRobotDirectionInPath(RobotDirection currentDirection, NodeDto[] nodes, EdgeDto[] edges)
|
|
{
|
|
RobotDirection[] RobotDirectionInNode = new RobotDirection[nodes.Length];
|
|
if (nodes.Length > 0) RobotDirectionInNode[0] = currentDirection;
|
|
if (nodes.Length > 2)
|
|
{
|
|
for (int i = 1; i < nodes.Length - 1; i++)
|
|
{
|
|
NodeDto nearLastNode = MapEditorHelper.GetNearByNode(nodes[i], nodes[i - 1], edges[i - 1], 0.1);
|
|
NodeDto nearFutureNode = MapEditorHelper.GetNearByNode(nodes[i], nodes[i + 1], edges[i], 0.1); ;
|
|
var angle = MapEditorHelper.GetAngle(nodes[i], nearLastNode, nearFutureNode);
|
|
if (angle < 89) RobotDirectionInNode[i] = RobotDirectionInNode[i - 1] == RobotDirection.FORWARD ? RobotDirection.BACKWARD : RobotDirection.FORWARD;
|
|
else RobotDirectionInNode[i] = RobotDirectionInNode[i - 1];
|
|
}
|
|
}
|
|
if (nodes.Length > 1) RobotDirectionInNode[^1] = RobotDirectionInNode[^2];
|
|
return RobotDirectionInNode;
|
|
}
|
|
|
|
public static double[] GetRobotThetaInPath(NodeDto[] nodes, EdgeDto[] edges)
|
|
{
|
|
if (nodes.Length < 2 || edges.Length < 1 || nodes.Length - 1 != edges.Length) return [];
|
|
double[] RobotThetaInNode = new double[nodes.Length];
|
|
if (nodes.Length > 1)
|
|
{
|
|
for (int i = 0; i < nodes.Length - 1; i++)
|
|
{
|
|
NodeDto nearFutureNode = MapEditorHelper.GetNearByNode(nodes[i], nodes[i + 1], edges[i], 0.1);
|
|
var angleForward = Math.Atan2(nearFutureNode.Y - nodes[i].Y, nearFutureNode.X - nodes[i].X) * 180 / Math.PI;
|
|
var angleBackward = Math.Atan2(nodes[i].Y - nearFutureNode.Y, nodes[i].X - nearFutureNode.X) * 180 / Math.PI;
|
|
|
|
RobotThetaInNode[i] = nodes[i].Direction == Direction.FORWARD ? angleForward : angleBackward;
|
|
}
|
|
RobotThetaInNode[^1] = RobotThetaInNode[^2];
|
|
}
|
|
return RobotThetaInNode;
|
|
}
|
|
|
|
public static NodeDto[] CalculatorDirection(NodeDto[] nodes, EdgeDto[] edges) => CalculatorDirection(RobotDirection.FORWARD, nodes, edges);
|
|
|
|
public static NodeDto[] CalculatorDirection(RobotDirection startDirection, NodeDto[] nodes, EdgeDto[] edges)
|
|
{
|
|
var directions = GetRobotDirectionInPath(startDirection, nodes, edges);
|
|
NodeDto[] returnNodes = [.. nodes];
|
|
for (int i = 0; i < returnNodes.Length; i++)
|
|
{
|
|
returnNodes[i].Direction = MapCompute.GetNodeDirection(directions[i]);
|
|
}
|
|
var thetas = GetRobotThetaInPath(returnNodes, edges);
|
|
for (int i = 0; i < returnNodes.Length; i++)
|
|
{
|
|
returnNodes[i].Theta = thetas[i];
|
|
}
|
|
return returnNodes;
|
|
}
|
|
|
|
public static NodeDto[] CalculatorDirection(double theta, NodeDto[] nodes, EdgeDto[] edges)
|
|
{
|
|
if (nodes.Length < 2) return nodes;
|
|
var RobotNearNode = new NodeDto()
|
|
{
|
|
X = nodes[0].X + Math.Cos(theta * Math.PI / 180),
|
|
Y = nodes[0].Y + Math.Sin(theta * Math.PI / 180),
|
|
};
|
|
nodes[0].Direction = MapEditorHelper.GetAngle(nodes[0], RobotNearNode, nodes[1]) > 89 ? MapShares.Enums.Direction.BACKWARD : MapShares.Enums.Direction.FORWARD;
|
|
var directions = GetRobotDirectionInPath(MapCompute.GetRobotDirection(nodes[0].Direction), nodes, edges);
|
|
NodeDto[] returnNodes = [.. nodes];
|
|
for (int i = 0; i < returnNodes.Length; i++)
|
|
{
|
|
returnNodes[i].Direction = MapCompute.GetNodeDirection(directions[i]);
|
|
}
|
|
var thetas = GetRobotThetaInPath(returnNodes, edges);
|
|
for (int i = 0; i < returnNodes.Length; i++)
|
|
{
|
|
returnNodes[i].Theta = thetas[i];
|
|
}
|
|
return returnNodes;
|
|
}
|
|
|
|
public static NodeDto[] CalculatorDirection(double theta, NodeDto[] nodes)
|
|
{
|
|
if (nodes.Length < 2) return nodes;
|
|
var RobotNearNode = new NodeDto()
|
|
{
|
|
X = nodes[0].X + Math.Cos(theta * Math.PI / 180),
|
|
Y = nodes[0].Y + Math.Sin(theta * Math.PI / 180),
|
|
};
|
|
nodes[0].Direction = MapEditorHelper.GetAngle(nodes[0], RobotNearNode, nodes[1]) > 89 ? MapShares.Enums.Direction.BACKWARD : MapShares.Enums.Direction.FORWARD;
|
|
for (int i = 1; i < nodes.Length - 1; i++)
|
|
{
|
|
if (nodes[i].X == nodes[i - 1].X && nodes[i].Y == nodes[i - 1].Y) nodes[i].Direction = nodes[i - 1].Direction;
|
|
else if (nodes[i].X == nodes[i + 1].X && nodes[i].Y == nodes[i + 1].Y) nodes[i].Direction = nodes[i - 1].Direction;
|
|
else
|
|
{
|
|
var angle = MapEditorHelper.GetAngle(nodes[i], nodes[i - 1], nodes[i + 1]);
|
|
nodes[i].Direction = angle > 89 ? nodes[i - 1].Direction : (nodes[i - 1].Direction == MapShares.Enums.Direction.FORWARD ? MapShares.Enums.Direction.BACKWARD : MapShares.Enums.Direction.FORWARD);
|
|
}
|
|
}
|
|
nodes[^1].Direction = nodes[^2].Direction;
|
|
return nodes;
|
|
}
|
|
|
|
public async Task<MessageResult<Dictionary<Guid, ZoneDto[]>>> GetZones(Guid mapId, NodeDto[] nodes)
|
|
{
|
|
var map = await MapManager.GetMapData(mapId);
|
|
if (!map.IsSuccess) return new(false, map.Message);
|
|
if (map.Data is null) return new(false, "Dữ liệu bản đồ có lỗi");
|
|
|
|
Dictionary<Guid, ZoneDto[]> NodeInZones = [];
|
|
foreach (var node in nodes)
|
|
{
|
|
List<ZoneDto> zones = [];
|
|
foreach(var zone in map.Data.Zones)
|
|
{
|
|
if (MapEditorHelper.IsPointInside(node.X, node.Y, zone)) zones.Add(zone);
|
|
}
|
|
if (zones.Count > 0) NodeInZones.Add(node.Id, [..zones]);
|
|
}
|
|
return new(true, "") { Data = NodeInZones };
|
|
}
|
|
|
|
public static ZoneDto[] GetZones(NodeDto[] nodes, Dictionary<Guid, ZoneDto[]> zones)
|
|
{
|
|
List<ZoneDto> returnZones = [];
|
|
foreach (var node in nodes)
|
|
{
|
|
if (zones.TryGetValue(node.Id, out ZoneDto[]? zone) && zone is not null && zone.Length > 0)
|
|
returnZones.AddRange(zone.Where(z => !returnZones.Any(zo => zo.Id == z.Id)));
|
|
}
|
|
return [.. returnZones];
|
|
}
|
|
}
|