266 lines
13 KiB
C#
266 lines
13 KiB
C#
using RobotNet.MapShares;
|
|
using RobotNet.MapShares.Dtos;
|
|
using RobotNet.MapShares.Enums;
|
|
using RobotNet.RobotManager.Services.Planner.AStar;
|
|
using RobotNet.RobotManager.Services.Planner.Space;
|
|
using RobotNet.RobotShares.Enums;
|
|
using RobotNet.Shares;
|
|
|
|
namespace RobotNet.RobotManager.Services.Planner.OmniDrive;
|
|
|
|
public class OmniDrivePathPlanner : IPathPlanner
|
|
{
|
|
private List<NodeDto> Nodes = [];
|
|
private List<EdgeDto> Edges = [];
|
|
private const double ReverseDirectionAngleDegree = 89;
|
|
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 EdgeDto[] GetEdgesPlanning(NodeDto[] nodes)
|
|
{
|
|
var EdgesPlanning = new List<EdgeDto>();
|
|
for (int i = 0; i < nodes.Length - 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}]");
|
|
if (Path.Data.Count == 1) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
|
|
|
|
var edgeplannings = GetEdgesPlanning([.. Path.Data]);
|
|
|
|
RobotDirection CurrenDirection = GetRobotDirection(Path.Data[0], Path.Data[1], edgeplannings[0], theta, true);
|
|
var FinalDirection = GetRobotDirectionInPath(CurrenDirection, Path.Data, [.. edgeplannings]);
|
|
foreach (var item in Path.Data)
|
|
{
|
|
item.Direction = MapCompute.GetNodeDirection(FinalDirection[Path.Data.IndexOf(item)]);
|
|
}
|
|
|
|
return new(true)
|
|
{
|
|
Data = ([.. Path.Data], [.. 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 basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
|
|
if (!basicPath.IsSuccess) return basicPath;
|
|
if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
|
|
|
|
RobotDirection goalDirection = GetRobotDirection(basicPath.Data.Nodes[^1], basicPath.Data.Nodes[^2], basicPath.Data.Edges[^1], goal.Theta, false);
|
|
if (MapCompute.GetNodeDirection(goalDirection) == basicPath.Data.Nodes[^1].Direction) return basicPath;
|
|
foreach (var node in basicPath.Data.Nodes)
|
|
{
|
|
node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
|
|
}
|
|
|
|
return basicPath;
|
|
}
|
|
|
|
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithStartDirection(double x, double y, double theta, Guid goalId, RobotDirection startDiretion = RobotDirection.NONE, CancellationToken? cancellationToken = null)
|
|
{
|
|
var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
|
|
if (!basicPath.IsSuccess) return basicPath;
|
|
if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
|
|
|
|
if (MapCompute.GetNodeDirection(startDiretion) == basicPath.Data.Nodes[0].Direction || startDiretion == RobotDirection.NONE) return basicPath;
|
|
foreach (var node in basicPath.Data.Nodes)
|
|
{
|
|
node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
|
|
}
|
|
|
|
return basicPath;
|
|
}
|
|
|
|
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithFinalDirection(double x, double y, double theta, Guid goalId, RobotDirection goalDirection = RobotDirection.NONE, CancellationToken? cancellationToken = null)
|
|
{
|
|
var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
|
|
if (!basicPath.IsSuccess) return basicPath;
|
|
if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
|
|
|
|
if (MapCompute.GetNodeDirection(goalDirection) == basicPath.Data.Nodes[^1].Direction || goalDirection == RobotDirection.NONE) return basicPath;
|
|
foreach (var node in basicPath.Data.Nodes)
|
|
{
|
|
node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
|
|
}
|
|
|
|
return basicPath;
|
|
}
|
|
|
|
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}]");
|
|
if (Path.Data.Length == 1) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
|
|
|
|
var edgeplannings = GetEdgesPlanning([.. Path.Data]);
|
|
|
|
RobotDirection CurrenDirection = GetRobotDirection(Path.Data[0], Path.Data[1], edgeplannings[0], theta, true);
|
|
var FinalDirection = GetRobotDirectionInPath(CurrenDirection, [.. Path.Data], [.. edgeplannings]);
|
|
for (int i = 0; i < Path.Data.Length; i++)
|
|
{
|
|
Path.Data[i].Direction = MapCompute.GetNodeDirection(FinalDirection[i]);
|
|
}
|
|
|
|
return new(true)
|
|
{
|
|
Data = ([.. Path.Data], [.. 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 basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
|
|
if (!basicPath.IsSuccess) return basicPath;
|
|
if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
|
|
|
|
RobotDirection goalDirection = GetRobotDirection(basicPath.Data.Nodes[^1], basicPath.Data.Nodes[^2], basicPath.Data.Edges[^1], goal.Theta, false);
|
|
if (MapCompute.GetNodeDirection(goalDirection) == basicPath.Data.Nodes[^1].Direction) return basicPath;
|
|
foreach (var node in basicPath.Data.Nodes)
|
|
{
|
|
node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
|
|
}
|
|
|
|
return basicPath;
|
|
}
|
|
|
|
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithStartDirection(Guid startNodeId, double theta, Guid goalId, RobotDirection startDiretion = RobotDirection.NONE, CancellationToken? cancellationToken = null)
|
|
{
|
|
var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
|
|
if (!basicPath.IsSuccess) return basicPath;
|
|
if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
|
|
|
|
if (MapCompute.GetNodeDirection(startDiretion) == basicPath.Data.Nodes[0].Direction || startDiretion == RobotDirection.NONE) return basicPath;
|
|
foreach (var node in basicPath.Data.Nodes)
|
|
{
|
|
node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
|
|
}
|
|
|
|
return basicPath;
|
|
}
|
|
|
|
public MessageResult<(NodeDto[] Nodes, EdgeDto[] Edges)> PathPlanningWithFinalDirection(Guid startNodeId, double theta, Guid goalId, RobotDirection goalDirection = RobotDirection.NONE, CancellationToken? cancellationToken = null)
|
|
{
|
|
var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
|
|
if (!basicPath.IsSuccess) return basicPath;
|
|
if (basicPath.Data.Nodes.Length < 2) return new(true, "Robot đang đứng ở điểm đích") { Data = ([], []) };
|
|
|
|
if (MapCompute.GetNodeDirection(goalDirection) == basicPath.Data.Nodes[^1].Direction || goalDirection == RobotDirection.NONE) return basicPath;
|
|
foreach (var node in basicPath.Data.Nodes)
|
|
{
|
|
node.Direction = node.Direction == Direction.FORWARD ? Direction.BACKWARD : Direction.FORWARD;
|
|
}
|
|
|
|
return basicPath;
|
|
}
|
|
}
|