Initial commit
This commit is contained in:
@@ -0,0 +1,380 @@
|
||||
using RobotNet.VDA5050.Order;
|
||||
using RobotNet10.Common;
|
||||
using RobotNet10.Common.Models;
|
||||
using RobotNet10.RobotApp.Services.Exceptions;
|
||||
using RobotNet10.RobotApp.Services.Robot;
|
||||
using RobotNet10.RobotApp.Services.Robot.Helper;
|
||||
using RobotNet10.RobotApp.Services.Robot.Models;
|
||||
using RobotNet10.RobotApp.Shared.Enums;
|
||||
|
||||
namespace RobotNet10.RobotApp.Services.Simulation.Algorithm;
|
||||
|
||||
public class PurePursuit
|
||||
{
|
||||
private double MaxAngularVelocity = 1.5;
|
||||
private double LookaheadDistance = 0.5;
|
||||
|
||||
private readonly double ResolutionSplit = 0.1;
|
||||
|
||||
private KDTree? KDTreeOrder;
|
||||
private KDTree? KDTreeWay;
|
||||
private OrderNode? LastNode;
|
||||
private Dictionary<string, (int start, int end)>? _segmentCache;
|
||||
|
||||
public OrderNode? LastOrderNode = null;
|
||||
private int LastNavNodeIndex = 0;
|
||||
public int OnNodeIndex = 0;
|
||||
public NavigationNode? Goal;
|
||||
public List<NavigationNode> Waypoints_Value = [];
|
||||
|
||||
public OrderNode[] OrderNodes = [];
|
||||
public OrderEdge[] OrderEdges = [];
|
||||
|
||||
public PurePursuit WithLookheadDistance(double distance)
|
||||
{
|
||||
LookaheadDistance = distance;
|
||||
return this;
|
||||
}
|
||||
|
||||
public PurePursuit WithMaxAngularVelocity(double vel)
|
||||
{
|
||||
MaxAngularVelocity = vel;
|
||||
return this;
|
||||
}
|
||||
|
||||
public PurePursuit WithPath(Node[] nodes, Edge[] edges, double currentTheta)
|
||||
{
|
||||
if (nodes.Length < 2) throw new SimulationException(RobotErrors.Error1002(nodes.Length));
|
||||
if (edges.Length < 1) throw new SimulationException();
|
||||
if (edges.Length != nodes.Length - 1) throw new SimulationException(RobotErrors.Error1004(nodes.Length, edges.Length));
|
||||
(OrderNodes, OrderEdges) = OrderConverter.Validate(nodes, edges, currentTheta);
|
||||
Waypoints_Value = [.. PathSplit(OrderNodes, OrderEdges)];
|
||||
BuildSegmentCache();
|
||||
KDTreeOrder = new KDTree(OrderNodes.Select(n => new KDTreeData(n.NodeId, n.X, n.Y)));
|
||||
KDTreeWay = new KDTree(Waypoints_Value.Select(n => new KDTreeData(n.Id.ToString(), n.X, n.Y)));
|
||||
return this;
|
||||
}
|
||||
|
||||
public void UpdateGoal(string goalId)
|
||||
{
|
||||
var goal = Waypoints_Value.FirstOrDefault(n => n.NodeId == goalId);
|
||||
if (goal is not null) Goal = goal;
|
||||
}
|
||||
|
||||
private NavigationNode[] PathSplit(OrderNode[] nodes, OrderEdge[] edges)
|
||||
{
|
||||
List<NavigationNode> navigationNode = [new()
|
||||
{
|
||||
Id = Guid.NewGuid(),
|
||||
NodeId = nodes[0].NodeId,
|
||||
X = nodes[0].X,
|
||||
Y = nodes[0].Y,
|
||||
Theta = nodes[0].Theta,
|
||||
Direction = edges[0].Direction,
|
||||
}];
|
||||
foreach (var edge in edges)
|
||||
{
|
||||
var startNode = nodes.FirstOrDefault(n => n.NodeId == edge.StartNodeId);
|
||||
var endNode = nodes.FirstOrDefault(n => n.NodeId == edge.EndNodeId);
|
||||
if (startNode is null) throw new PathPlannerException(RobotErrors.Error1008(edge.EdgeId, edge.StartNodeId));
|
||||
if (endNode is null) throw new PathPlannerException(RobotErrors.Error1009(edge.EdgeId, edge.EndNodeId));
|
||||
|
||||
var spaceEdge = new SpaceEdge()
|
||||
{
|
||||
StartX = startNode.X,
|
||||
StartY = startNode.Y,
|
||||
EndX = endNode.X,
|
||||
EndY = endNode.Y,
|
||||
ControlPoint1X = edge.ControlPoint1X ?? 0,
|
||||
ControlPoint1Y = edge.ControlPoint1Y ?? 0,
|
||||
ControlPoint2X = edge.ControlPoint2X ?? 0,
|
||||
ControlPoint2Y = edge.ControlPoint2Y ?? 0,
|
||||
Degree = edge.Degree,
|
||||
};
|
||||
|
||||
double length = SpaceCompute.GetEdgeLength(spaceEdge, ResolutionSplit);
|
||||
if (length <= 0) continue;
|
||||
double step = ResolutionSplit / length;
|
||||
|
||||
for (double t = step; t <= 1 - step; t += step)
|
||||
{
|
||||
(double x, double y) = SpaceCompute.BezierPoint(t, spaceEdge);
|
||||
navigationNode.Add(new()
|
||||
{
|
||||
Id = Guid.NewGuid(),
|
||||
NodeId = string.Empty,
|
||||
X = x,
|
||||
Y = y,
|
||||
Theta = null,
|
||||
Direction = edge.Direction,
|
||||
});
|
||||
}
|
||||
navigationNode.Add(new()
|
||||
{
|
||||
Id = Guid.NewGuid(),
|
||||
NodeId = endNode.NodeId,
|
||||
X = endNode.X,
|
||||
Y = endNode.Y,
|
||||
Theta = endNode.Theta,
|
||||
Direction = edge.Direction,
|
||||
});
|
||||
}
|
||||
return [.. navigationNode];
|
||||
}
|
||||
|
||||
private void BuildSegmentCache()
|
||||
{
|
||||
_segmentCache = [];
|
||||
|
||||
for (int i = 0; i < OrderNodes.Length - 1; i++)
|
||||
{
|
||||
var currentNodeId = OrderNodes[i].NodeId;
|
||||
|
||||
// Xác định phạm vi: từ (i-1) đến (i+1)
|
||||
int rangeStart = Math.Max(0, i - 1);
|
||||
int rangeEnd = Math.Min(OrderNodes.Length - 1, i + 1);
|
||||
|
||||
var startNodeId = OrderNodes[rangeStart].NodeId;
|
||||
var endNodeId = OrderNodes[rangeEnd].NodeId;
|
||||
|
||||
// Tìm indices trong Waypoints_Value
|
||||
int waypointStartIdx = Waypoints_Value.FindIndex(n => n.NodeId == startNodeId);
|
||||
int waypointEndIdx = Waypoints_Value.FindIndex(n => n.NodeId == endNodeId);
|
||||
|
||||
// Xử lý trường hợp không tìm thấy
|
||||
if (waypointStartIdx == -1) waypointStartIdx = 0;
|
||||
if (waypointEndIdx == -1) waypointEndIdx = Waypoints_Value.Count - 1;
|
||||
|
||||
// Cache theo NodeId của OrderNode
|
||||
_segmentCache[currentNodeId] = (waypointStartIdx, waypointEndIdx);
|
||||
}
|
||||
}
|
||||
|
||||
private (OrderNode node, int index)? GetOnNodeWithKDTree(double x, double y)
|
||||
{
|
||||
KDTreeOrder ??= new KDTree(OrderNodes.Select(n => new KDTreeData(n.NodeId, n.X, n.Y)));
|
||||
LastNode ??= OrderNodes[0];
|
||||
var dx = LastNode.X - x;
|
||||
var dy = LastNode.Y - y;
|
||||
var minDistance = Math.Sqrt(dx * dx + dy * dy);
|
||||
var closesFindedNode = KDTreeOrder.FindNearest(x, y, minDistance);
|
||||
if (closesFindedNode == null) return null;
|
||||
|
||||
var closesNodeIndex = Array.FindIndex(OrderNodes, n => n.NodeId == closesFindedNode?.Id);
|
||||
if (closesNodeIndex == -1) return null;
|
||||
|
||||
if (OrderNodes[closesNodeIndex].NodeId != LastNode.NodeId)
|
||||
{
|
||||
var skipIndex = closesNodeIndex == 0 ? 0 : closesNodeIndex - 1;
|
||||
var newNodes = OrderNodes.Skip(skipIndex);
|
||||
KDTreeOrder = new KDTree(newNodes.Select(n => new KDTreeData(n.NodeId, n.X, n.Y)));
|
||||
|
||||
LastNode = OrderNodes[closesNodeIndex];
|
||||
}
|
||||
return (OrderNodes[closesNodeIndex], closesNodeIndex);
|
||||
}
|
||||
|
||||
private (OrderNode node, int index) GetOnNode(double x, double y)
|
||||
{
|
||||
LastNode ??= OrderNodes[0];
|
||||
var lastIndex = Array.FindIndex(OrderNodes, n => n.NodeId == LastNode?.NodeId);
|
||||
lastIndex = Math.Max(0, lastIndex);
|
||||
|
||||
var dx = LastNode.X - x;
|
||||
var dy = LastNode.Y - y;
|
||||
var minDistance = Math.Sqrt(dx * dx + dy * dy);
|
||||
|
||||
OrderNode onNode = LastNode;
|
||||
int index = 0;
|
||||
for (int i = lastIndex; i < OrderNodes.Length; i++)
|
||||
{
|
||||
var node = OrderNodes[i]; // FIX: Dùng đúng array
|
||||
dx = x - node.X;
|
||||
dy = y - node.Y;
|
||||
var distance = dx * dx + dy * dy;
|
||||
if (distance < minDistance)
|
||||
{
|
||||
onNode = OrderNodes[i];
|
||||
minDistance = distance;
|
||||
index = i;
|
||||
}
|
||||
}
|
||||
return (onNode, index);
|
||||
}
|
||||
|
||||
public (NavigationNode node, int index) GetOnNavNode(double x, double y)
|
||||
{
|
||||
double minDistance = double.MaxValue;
|
||||
NavigationNode onNode = Waypoints_Value[0];
|
||||
int index = 0;
|
||||
for (int i = 1; i < Waypoints_Value.Count; i++)
|
||||
{
|
||||
var distance = Math.Sqrt(Math.Pow(x - Waypoints_Value[i].X, 2) + Math.Pow(y - Waypoints_Value[i].Y, 2));
|
||||
if (distance < minDistance)
|
||||
{
|
||||
onNode = Waypoints_Value[i];
|
||||
minDistance = distance;
|
||||
index = i;
|
||||
}
|
||||
}
|
||||
return (onNode, index);
|
||||
}
|
||||
|
||||
private (NavigationNode? node, int index) OnNodeLinear(double x, double y)
|
||||
{
|
||||
var (node, index) = GetOnNodeWithKDTree(x, y) ?? GetOnNode(x, y);
|
||||
|
||||
int waypointStartIdx = 0;
|
||||
int waypointEndIdx = Waypoints_Value.Count - 1;
|
||||
if (_segmentCache is null)
|
||||
{
|
||||
var startIndex = Math.Max(0, index - 1);
|
||||
var endIndex = Math.Min(OrderNodes.Length - 1, index + 1);
|
||||
var navStartNodeIndex = Waypoints_Value.FindIndex(n => n.NodeId == OrderNodes[startIndex].NodeId);
|
||||
waypointStartIdx = navStartNodeIndex == -1 ? 0 : navStartNodeIndex;
|
||||
var navEndNodeIndex = Waypoints_Value.FindIndex(n => n.NodeId == OrderNodes[endIndex].NodeId);
|
||||
waypointEndIdx = navEndNodeIndex == -1 ? Waypoints_Value.Count - 1 : navEndNodeIndex;
|
||||
}
|
||||
else if (_segmentCache.TryGetValue(node.NodeId, out var range)) (waypointStartIdx, waypointEndIdx) = range;
|
||||
|
||||
waypointStartIdx = Math.Min(waypointStartIdx, OnNodeIndex);
|
||||
|
||||
var dx = OrderNodes[index].X - x;
|
||||
var dy = OrderNodes[index].Y - y;
|
||||
var minDistance = Math.Sqrt(dx * dx + dy * dy);
|
||||
|
||||
NavigationNode? bestNode = null;
|
||||
int bestIndex = -1;
|
||||
|
||||
for (int i = waypointStartIdx; i <= waypointEndIdx; i++)
|
||||
{
|
||||
var orderNodeId = Waypoints_Value[i].NodeId;
|
||||
|
||||
// Tìm NavigationNode tương ứng
|
||||
var navIndex = Waypoints_Value.FindIndex(n => n.NodeId == orderNodeId);
|
||||
if (navIndex == -1) continue;
|
||||
|
||||
var navNode = Waypoints_Value[navIndex];
|
||||
dx = x - navNode.X;
|
||||
dy = y - navNode.Y;
|
||||
var distanceSq = dx * dx + dy * dy;
|
||||
|
||||
if (distanceSq < minDistance)
|
||||
{
|
||||
minDistance = distanceSq;
|
||||
bestNode = navNode;
|
||||
bestIndex = navIndex;
|
||||
}
|
||||
}
|
||||
|
||||
return (bestNode, bestIndex);
|
||||
}
|
||||
|
||||
private (NavigationNode? node, int index) OnNodeWithKDTree(double x, double y)
|
||||
{
|
||||
KDTreeWay ??= new KDTree(Waypoints_Value.Select(n => new KDTreeData(n.Id.ToString(), n.X, n.Y)));
|
||||
var kdtreeNode = KDTreeWay.FindNearest(x, y, double.MaxValue);
|
||||
if(kdtreeNode is not null && Guid.TryParse(kdtreeNode.Id, out Guid kdtreeNodeId))
|
||||
{
|
||||
var nodeIndex = Waypoints_Value.FindIndex(n => n.Id == kdtreeNodeId);
|
||||
if(nodeIndex != -1) return (Waypoints_Value[nodeIndex], nodeIndex);
|
||||
}
|
||||
return (null, -1);
|
||||
}
|
||||
|
||||
private (NavigationNode? node, int index) OnNode(double x, double y)
|
||||
{
|
||||
var (node, index) = OnNodeWithKDTree(x, y);
|
||||
if (node is null || index == -1) return OnNodeLinear(x, y);
|
||||
return (node, index);
|
||||
}
|
||||
|
||||
private void UpdateLastOrderNode()
|
||||
{
|
||||
var oldNavNodes = Waypoints_Value.Skip(LastNavNodeIndex).Take(OnNodeIndex);
|
||||
var lastNavNode = oldNavNodes.LastOrDefault(n => string.IsNullOrEmpty(n.NodeId));
|
||||
if (lastNavNode is null) return;
|
||||
LastOrderNode = OrderNodes.FirstOrDefault(n => n.NodeId == lastNavNode.NodeId);
|
||||
if (LastOrderNode is not null) LastNavNodeIndex = Waypoints_Value.IndexOf(lastNavNode);
|
||||
}
|
||||
|
||||
public double PurePursuit_step(double X_Ref, double Y_Ref, double Angle_Ref)
|
||||
{
|
||||
if (Waypoints_Value is null || Waypoints_Value.Count < 2) return 0;
|
||||
NavigationNode? lookaheadStartPt = null;
|
||||
var (onNode, index) = OnNode(X_Ref, Y_Ref);
|
||||
if (onNode is null || Goal is null) return 0;
|
||||
OnNodeIndex = index;
|
||||
UpdateLastOrderNode();
|
||||
double lookDistance = 0;
|
||||
for (int i = OnNodeIndex + 1; i < Waypoints_Value.IndexOf(Goal); i++)
|
||||
{
|
||||
lookDistance += Math.Sqrt(Math.Pow(Waypoints_Value[i - 1].X - Waypoints_Value[i].X, 2) + Math.Pow(Waypoints_Value[i - 1].Y - Waypoints_Value[i].Y, 2));
|
||||
if (lookDistance >= LookaheadDistance || Waypoints_Value[i].Direction != onNode.Direction)
|
||||
{
|
||||
lookaheadStartPt = Waypoints_Value[i];
|
||||
break;
|
||||
}
|
||||
}
|
||||
lookaheadStartPt ??= Goal;
|
||||
if (onNode.Direction == RobotDirection.BACKWARD)
|
||||
{
|
||||
if (Angle_Ref > Math.PI) Angle_Ref -= Math.PI * 2;
|
||||
else if (Angle_Ref < -Math.PI) Angle_Ref += Math.PI * 2;
|
||||
Angle_Ref += Math.PI;
|
||||
if (Angle_Ref > Math.PI) Angle_Ref -= Math.PI * 2;
|
||||
}
|
||||
var distance = Math.Atan2(lookaheadStartPt.Y - Y_Ref, lookaheadStartPt.X - X_Ref) - Angle_Ref;
|
||||
|
||||
if (Math.Abs(distance) > Math.PI)
|
||||
{
|
||||
double minDistance;
|
||||
if (distance + Math.PI == 0.0) minDistance = 0.0;
|
||||
else
|
||||
{
|
||||
double data = (distance + Math.PI) / (2 * Math.PI);
|
||||
if (data < 0) data = Math.Round(data + 0.5);
|
||||
else data = Math.Round(data - 0.5);
|
||||
minDistance = distance + Math.PI - data * (2 * Math.PI);
|
||||
double checker = 0;
|
||||
if (minDistance != 0.0)
|
||||
{
|
||||
checker = Math.Abs((distance + Math.PI) / (2 * Math.PI));
|
||||
}
|
||||
if (!(Math.Abs(checker - Math.Floor(checker + 0.5)) > 2.2204460492503131E-16 * checker))
|
||||
{
|
||||
minDistance = 0.0;
|
||||
}
|
||||
else if (distance + Math.PI < 0.0)
|
||||
{
|
||||
minDistance += Math.PI * 2;
|
||||
}
|
||||
}
|
||||
if (minDistance == 0.0 && distance + Math.PI > 0.0)
|
||||
{
|
||||
minDistance = Math.PI * 2;
|
||||
}
|
||||
distance = minDistance - Math.PI;
|
||||
}
|
||||
|
||||
var AngularVelocity = 2.0 * 0.5 * Math.Sin(distance) / LookaheadDistance;
|
||||
if (Math.Abs(AngularVelocity) > MaxAngularVelocity)
|
||||
{
|
||||
if (AngularVelocity < 0.0)
|
||||
{
|
||||
AngularVelocity = -1.0;
|
||||
}
|
||||
else if (AngularVelocity > 0.0)
|
||||
{
|
||||
AngularVelocity = 1.0;
|
||||
}
|
||||
else if (AngularVelocity == 0.0)
|
||||
{
|
||||
AngularVelocity = 0.0;
|
||||
}
|
||||
AngularVelocity *= MaxAngularVelocity;
|
||||
}
|
||||
return AngularVelocity;
|
||||
}
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user