Hi,
Can someone explain how the code relate to Pure pursuit algorithm?
Why need to calculate point on ABLine closest to current position?
// ** Pure pursuit ** - calc point on ABLine closest to current position
double U = (((pivot.easting - currentABLineP1.easting) * dx)
+ ((pivot.northing - currentABLineP1.northing) * dy))
/ ((dx * dx) + (dy * dy));
Is there any picture to show the algorithm with the parameters in the code? It will give me a better understanding of the algorithm.
// ** Pure pursuit ** - calc point on ABLine closest to current position
double U = (((pivot.easting - currentABLineP1.easting) * dx)
+ ((pivot.northing - currentABLineP1.northing) * dy))
/ ((dx * dx) + (dy * dy));
//point on AB line closest to pivot axle point
rEastAB = currentABLineP1.easting + (U * dx);
rNorthAB = currentABLineP1.northing + (U * dy);
//update base on autosteer settings and distance from line
double goalPointDistance = mf.vehicle.UpdateGoalPointDistance();
if (mf.isReverse ? isHeadingSameWay : !isHeadingSameWay)
{
goalPointAB.easting = rEastAB - (Math.Sin(abHeading) * goalPointDistance);
goalPointAB.northing = rNorthAB - (Math.Cos(abHeading) * goalPointDistance);
}
else
{
goalPointAB.easting = rEastAB + (Math.Sin(abHeading) * goalPointDistance);
goalPointAB.northing = rNorthAB + (Math.Cos(abHeading) * goalPointDistance);
}
//calc "D" the distance from pivot axle to lookahead point
double goalPointDistanceDSquared
= glm.DistanceSquared(goalPointAB.northing, goalPointAB.easting, pivot.northing, pivot.easting);
//calculate the the new x in local coordinates and steering angle degrees based on wheelbase
double localHeading;
if (isHeadingSameWay) localHeading = glm.twoPI - mf.fixHeading + inty;
else localHeading = glm.twoPI - mf.fixHeading - inty;
ppRadiusAB = goalPointDistanceDSquared / (2 * (((goalPointAB.easting - pivot.easting) * Math.Cos(localHeading))
+ ((goalPointAB.northing - pivot.northing) * Math.Sin(localHeading))));
steerAngleAB = glm.toDegrees(Math.Atan(2 * (((goalPointAB.easting - pivot.easting) * Math.Cos(localHeading))
+ ((goalPointAB.northing - pivot.northing) * Math.Sin(localHeading))) * mf.vehicle.wheelbase
/ goalPointDistanceDSquared));
Thanks a lot!