DungTT/RobotNet
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first commit -push

Browse Source
This commit is contained in:
dungtt
2025-10-15 15:15:53 +07:00
parent 674ae395be
commit a9577c5756
885 changed files with 74595 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

266
RobotNet.RobotManager/Services/Planner/Differential/DifferentialPathPlanner.cs Normal file
View File

@@ -0,0 +1,266 @@
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.Differential;
public class DifferentialPathPlanner : 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 = ([goal], []) };
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 = ([goal], []) };
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;
}
}