ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

# Checking for GPS waypoint being passed?

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], double next_coords[NUMBER_OF_WAYPOINTS], uint32_t &wp) { // This is only for N and W hemispheres

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

if (((next_coords[wp + 1] - current_coords[wp]) < 0) and ((next_coords[wp + 1] - current_coords[wp]) > 0)) {
wp++;
}
}

if (((next_coords[wp + 1] - current_coords[wp]) > 0) and ((next_coords[wp + 1] - current_coords[wp]) > 0)) {
wp++;
}
}

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

if (((next_coords[wp + 1] - current_coords[wp]) < 0) and ((next_coords[wp + 1] - current_coords[wp]) < 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? edit retag close merge delete

Sort by » oldest newest most voted 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.

more