QUYVN/I150
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
QUYVN
2026-07-03 16:37:12 +07:00
commit 63b8c1ea8b
1931 changed files with 640587 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

202
srcs/RobotNet10/Commons/RobotNet10.GlobalPathPlanner/Differential/DifferentialPlanner.cs Normal file
View File

@@ -0,0 +1,202 @@
using RobotNet10.GlobalPathPlanner.AStar;
using RobotNet10.GlobalPathPlanner.Model;
namespace RobotNet10.GlobalPathPlanner.Differential;
public class DifferentialPlanner : IPathPlanner
{
private List<GlobalNode> Nodes = [];
private List<GlobalEdge> Edges = [];
private const double Ratio = 0.1;
private PathPlannerOptions Options = new()
{
LimitDistanceToEdge = 2,
LimitDistanceToNode = 0.3,
ResolutionSplit = 0.1,
ChangeOrientationAngle = 89
};
public void SetData(GlobalNode[] nodes, GlobalEdge[] edges)
{
Nodes = [.. nodes];
Edges = [.. edges];
}
public void SetOptions(PathPlannerOptions options)
{
Options = options;
}
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanning(double x, double y, double theta, Guid goalId, CancellationToken? cancellationToken = null)
{
var goal = Nodes.FirstOrDefault(n => n.Id == goalId) ?? throw new Exception($"Destination {goalId} does not exist in the map");
using CancellationTokenSource cancellation = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken ?? CancellationToken.None);
if(Options.TimeOut != null && Options.TimeOut.HasValue) cancellation.CancelAfter(Options.TimeOut.Value);
try
{
var AStarPathPlanner = new AStarPlanner(Nodes, Edges);
(var path, var closesEdge) = AStarPathPlanner.Planning(x, y,
new GlobalNode() { Id = goal.Id, Name = goal.Name, X = goal.X, Y = goal.Y },
Options.LimitDistanceToEdge,
Options.LimitDistanceToNode,
cancellation.Token);
if (path is null || path.Length < 1) throw new Exception($"The path to {goal.Name} - {goal.Id} does not exist from [{x}, {y}, {theta}]");
if (path.Length == 1) return (path, []);
var edgeplannings = MathExtensions.GetEdgesPlanning([.. path], [.. Edges], closesEdge);
Orientation CurrenDirection = MathExtensions.GetOrientationStart(path[0], path[1], edgeplannings[0], theta, Ratio, Options.ChangeOrientationAngle);
var FinalDirection = MathExtensions.GetOrientations(CurrenDirection, path, edgeplannings, Ratio, Options.ChangeOrientationAngle);
for (int i = 0; i < path.Length; i++)
{
path[i] = new GlobalNode
{
Id = path[i].Id,
X = path[i].X,
Y = path[i].Y,
Name = path[i].Name,
MapId = path[i].MapId,
Orientation = FinalDirection[i]
};
}
return ([.. path], [.. edgeplannings]);
}
catch (OperationCanceledException)
{
if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) throw new OperationCanceledException();
else throw new TimeoutException();
}
}
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithStartDirection(double x, double y, double theta, Guid goalId, Orientation startDiretion = Orientation.NONE, CancellationToken? cancellationToken = null)
{
var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
if (basicPath.Nodes.Length < 1) return basicPath;
if (startDiretion == basicPath.Nodes[0].Orientation || startDiretion == Orientation.NONE) return basicPath;
foreach (var node in basicPath.Nodes)
{
node.Orientation = node.Orientation == Orientation.FORWARD ? Orientation.BACKWARD : Orientation.FORWARD;
}
return basicPath;
}
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithFinalDirection(double x, double y, double theta, Guid goalId, Orientation goalDirection = Orientation.NONE, CancellationToken? cancellationToken = null)
{
var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
if (basicPath.Nodes.Length < 1) return basicPath;
if (goalDirection == basicPath.Nodes[^1].Orientation || goalDirection == Orientation.NONE) return basicPath;
foreach (var node in basicPath.Nodes)
{
node.Orientation = node.Orientation == Orientation.FORWARD ? Orientation.BACKWARD : Orientation.FORWARD;
}
return basicPath;
}
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithAngle(double x, double y, double theta, Guid goalId, double goalAngle, CancellationToken? cancellationToken = null)
{
var basicPath = PathPlanning(x, y, theta, goalId, cancellationToken);
if (basicPath.Nodes.Length < 1) return basicPath;
Orientation goalDirection = MathExtensions.GetOrientationEnd(basicPath.Nodes[^1], basicPath.Nodes[^2], basicPath.Edges[^1], goalAngle, Ratio, Options.ChangeOrientationAngle);
if (goalDirection == basicPath.Nodes[^1].Orientation) return basicPath;
foreach (var node in basicPath.Nodes)
{
node.Orientation = node.Orientation == Orientation.FORWARD ? Orientation.BACKWARD : Orientation.FORWARD;
}
return basicPath;
}
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanning(Guid startNodeId, double theta, Guid goalId, CancellationToken? cancellationToken = null)
{
var goal = Nodes.FirstOrDefault(n => n.Id == goalId) ?? throw new Exception($"Destination {goalId} does not exist in the map");
var startNode = Nodes.FirstOrDefault(n => n.Id == startNodeId) ?? throw new Exception($"Starting node {startNodeId} does not exist in the map");
using CancellationTokenSource cancellation = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken ?? CancellationToken.None);
if (Options.TimeOut != null && Options.TimeOut.HasValue) cancellation.CancelAfter(Options.TimeOut.Value);
try
{
var AStarPathPlanner = new AStarPlanner(Nodes, Edges);
(var path, var closesEdge) = AStarPathPlanner.Planning(startNode, goal, cancellation.Token);
if (path is null || path.Length < 1) throw new Exception($"The path to {goal.Name} - {goal.Id} does not exist from [{startNode.Name} - {startNode.Id}]");
if (path.Length == 1) return (path, []);
var edgeplannings = MathExtensions.GetEdgesPlanning([.. path], [.. Edges], closesEdge);
Orientation CurrenDirection = MathExtensions.GetOrientationStart(path[0], path[1], edgeplannings[0], theta, Ratio, Options.ChangeOrientationAngle);
var FinalDirection = MathExtensions.GetOrientations(CurrenDirection, path, edgeplannings, Ratio, Options.ChangeOrientationAngle);
for (int i = 0; i < path.Length; i++)
{
path[i] = new GlobalNode
{
Id = path[i].Id,
X = path[i].X,
Y = path[i].Y,
Name = path[i].Name,
MapId = path[i].MapId,
Orientation = FinalDirection[i]
};
}
return ([.. path], [.. edgeplannings]);
}
catch(OperationCanceledException)
{
if (cancellationToken is not null && cancellationToken.Value.IsCancellationRequested) throw new OperationCanceledException();
else throw new TimeoutException();
}
}
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithStartDirection(Guid startNodeId, double theta, Guid goalId, Orientation startDiretion = Orientation.NONE, CancellationToken? cancellationToken = null)
{
var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
if (basicPath.Nodes.Length < 1) return basicPath;
if (startDiretion == basicPath.Nodes[0].Orientation || startDiretion == Orientation.NONE) return basicPath;
foreach (var node in basicPath.Nodes)
{
node.Orientation = node.Orientation == Orientation.FORWARD ? Orientation.BACKWARD : Orientation.FORWARD;
}
return basicPath;
}
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithFinalDirection(Guid startNodeId, double theta, Guid goalId, Orientation goalDirection = Orientation.NONE, CancellationToken? cancellationToken = null)
{
var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
if (basicPath.Nodes.Length < 1) return basicPath;
if (goalDirection == basicPath.Nodes[^1].Orientation || goalDirection == Orientation.NONE) return basicPath;
foreach (var node in basicPath.Nodes)
{
node.Orientation = node.Orientation == Orientation.FORWARD ? Orientation.BACKWARD : Orientation.FORWARD;
}
return basicPath;
}
public (GlobalNode[] Nodes, GlobalEdge[] Edges) PathPlanningWithAngle(Guid startNodeId, double theta, Guid goalId, double goalAngle, CancellationToken? cancellationToken = null)
{
var basicPath = PathPlanning(startNodeId, theta, goalId, cancellationToken);
if (basicPath.Nodes.Length < 1) return basicPath;
Orientation goalDirection = MathExtensions.GetOrientationEnd(basicPath.Nodes[^1], basicPath.Nodes[^2], basicPath.Edges[^1], goalAngle, Ratio, Options.ChangeOrientationAngle);
if (goalDirection == basicPath.Nodes[^1].Orientation) return basicPath;
foreach (var node in basicPath.Nodes)
{
node.Orientation = node.Orientation == Orientation.FORWARD ? Orientation.BACKWARD : Orientation.FORWARD;
}
return basicPath;
}
}