Checking for GPS waypoint being passed?

Question

I am working on an autonomous vehicle system that navigates and avoids obstacles.

Navigation alone, it goes from waypoint to waypoint by hitting the circular threshold points, basically saying "when within this distance, increment the target waypoint.

Now, when there is an obstacle and the threshold point is "inside" the obstacle, 99.9% of the times the threshold point won't be hit as the is threshold is set to very low due to the system using RTK. I am trying to use two intersecting lines (lat and long) to check whether or not the current waypoint has passed the next one, however that doesn't seem to work very well and I can't wrap my head around how to do it. Here's my code (in the 2D array, in the second dimension 0 is for lat and 1 is for long):

void trackWaypoint(double heading, double current_coords[NUMBER_OF_WAYPOINTS][2], double next_coords[NUMBER_OF_WAYPOINTS][2], uint32_t &wp) { // This is only for N and W hemispheres

if (wp != (NUMBER_OF_WAYPOINTS - 1)) {

    if ((heading >= 0) and (heading < 90)) {
        
        if (((next_coords[wp + 1][0] - current_coords[wp][0]) < 0) and ((next_coords[wp + 1][1] - current_coords[wp][1]) > 0)) {
            wp++;
        }
    }
    else if ((heading >= 90) and (heading < 180)) {
        
        if (((next_coords[wp + 1][0] - current_coords[wp][0]) > 0) and ((next_coords[wp + 1][1] - current_coords[wp][1]) > 0)) {
            wp++;
        }
    }
    else if ((heading >= 180) and (heading < 270)) {
        
        if (((next_coords[wp + 1][0] - current_coords[wp][0]) > 0) and ((next_coords[wp + 1][1] - current_coords[wp][1]) < 0)) {
            wp++;
        }
    }
    else { // [270, 360)
        
        if (((next_coords[wp + 1][0] - current_coords[wp][0]) < 0) and ((next_coords[wp + 1][1] - current_coords[wp][1]) < 0)) {
            wp++;
        }
    }
}

}

Here's two scenarios, A and B, assuming we are operating in Northern and Western hemispheres (sorry for lousy drawing):

In A, if the system decides to take the left path, it will not be able to to determine whether or not the next waypoint has been passed due to it thinking it hasn't intersected along the longitude (x) axis.

In A, if the system decides to take the upper path, it will not be able to to determine whether or not the next waypoint has been passed due to it thinking it hasn't intersected along the latitude (y) axis.

Is there a different way of doing it? If not, how could I account for both paths passing the next waypoint?

---

Originally posted by Hypomania on ROS Answers with karma: 120 on 2019-01-14

Post score: 0

Answer 1

Instead of trying to solve this problem in terms of X and Y (Lat and Long) You want to use some more relevant geometry. Referring to your diagram above, if you define an infinite line through the next two green waypoints. Then you can calculate the position of the closest point on that line to your current position. If this position is before the first green waypoint then it hasn't been passed, however if that closest point is beyond the first green waypoint then the point has been passed and you can move to the next one. The maths for this is surprisingly simple using vectors:

Define three vectors:

• P the current location
• W1 waypoint 1
• W2 waypoint 2

Calculate the unit direction (D) between W2 and W1

• D = (W2 - W1) / |W2 - W1|

then distance in front or behind W1 (Dist)

• Dist = (P - W1).dot(D)

This will avoid the orientation problems you described. However this may mean that you could potentially drift a long way from the waypoints if there are a lot of obstacles. Hope this helps though.

---

Originally posted by PeteBlackerThe3rd with karma: 9529 on 2019-01-14

This answer was ACCEPTED on the original site

Post score: 2

---

Original comments

Comment by Hypomania on 2019-01-14:
Thank you so much, really appreciated. That does make sense, I am gonna try to see if I can implement that!

Comment by PeteBlackerThe3rd on 2019-01-14:
No problem.