How to find current waypoint index of path in example of "Path Following for a Differential Drive Robot"?

I wanted to figure out the current waypoint during the simulation. I have understood that the waypoint was consecutively updated as the vehicle is reached on it. But I could not find any variable represented the current waypoint number(index).

Answer 1

[Remo Pillat]

Hi Changho,

The example uses the controllerPurePursuit object to execute the path following. Even though you provide a list of waypoints to the object, the algorithm does not keep track of the "current" waypoint, but simply calculates the appropriate lookahead point on the continuous path along all waypoints. This has two advantages: (1) the vehicle can start anywhere on the path, not just at the beginning, and (2) it makes for a more robust controller if waypoints are not hit exactly. The details of the algorithm are explained on this page.

The current lookahead point is returned as the third output argument, so you could use that to calculate the proximity to the waypoints yourself. If you want to access the lookahead point, you can change the line:

[v, omega] = controller(robotCurrentPose);

to

[v, omega, lookaheadPoint] = controller(robotCurrentPose);

Thanks, Remo