381 lines
14 KiB
C#
381 lines
14 KiB
C#
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;
|
|
}
|
|
|
|
}
|