157 lines
5.5 KiB
C#
157 lines
5.5 KiB
C#
//namespace RobotApp.Services.Navigation.Algorithm;
|
|
//public class PurePursuit
|
|
//{
|
|
// private double MaxAngularVelocity = 1.5;
|
|
// private double LookaheadDistance = 0.5;
|
|
// private List<NavigationNode> Waypoints_Value = [];
|
|
// public int OnNodeIndex = 0;
|
|
// public int GoalIndex = 0;
|
|
// public NavigationNode? Goal;
|
|
|
|
// public PurePursuit WithLookheadDistance(double distance)
|
|
// {
|
|
// LookaheadDistance = distance;
|
|
// return this;
|
|
// }
|
|
|
|
// public PurePursuit WithMaxAngularVelocity(double vel)
|
|
// {
|
|
// MaxAngularVelocity = vel;
|
|
// return this;
|
|
// }
|
|
|
|
// public PurePursuit WithPath(NavigationNode[] path)
|
|
// {
|
|
// Waypoints_Value = [.. path];
|
|
// return this;
|
|
// }
|
|
|
|
// public void UpdatePath(NavigationNode[] path)
|
|
// {
|
|
// Waypoints_Value = [.. path];
|
|
// }
|
|
|
|
// public void UpdateGoal(NavigationNode goal)
|
|
// {
|
|
// Goal = goal;
|
|
// GoalIndex = Waypoints_Value.IndexOf(Goal);
|
|
// }
|
|
|
|
// public (NavigationNode node, int index) GetOnNode(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) OnNode(double x, double y)
|
|
// {
|
|
// if (Waypoints_Value is null || Waypoints_Value.Count == 0) return (null, 0);
|
|
// double minDistance = double.MaxValue;
|
|
// int index = 0;
|
|
// NavigationNode? onNode = null;
|
|
// if (Goal is null) return (onNode, index);
|
|
// for (int i = OnNodeIndex; i < Waypoints_Value.IndexOf(Goal); 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);
|
|
// }
|
|
|
|
// 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;
|
|
// 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;
|
|
// }
|
|
|
|
//}
|