autopilot jumps to future waypoints.

[tmagcaya]

Solved! See New Edits Below For Progress!

I am trying to get the ardrone to follow the path below:

But while flying the ardrone jumps to a future waypoint and continues from there randomly.
Here is an example path that it took:

I analyzed the way-points that it sent to the autopilot and noticed that in one flight it went from these two points:
New Target: xyz = -2.062, -2.289, 1.905, yaw=8.581
New Target: xyz = 4.375, -0.039, 1.905, yaw=8.581

Whereas those points, looking at the waypoint file, correspond to lines 48 and 131. So the autopilot skipped the points in between.

Line 48 of Waypoint file: goto -2.25 -2.25 2.0 0
Line 131 of Waypoint file: goto 4.75 0 2.0 0

How should I fix this issue?
Is there a known place in the code where the autopilot calculates the new waypoint? or perhaps a place where it reads the next waypoint?

EDIT 1:
I first though that the minPublishFrequency was causing the issue since state estimator gave the following message: "0.246477s between two consecutive navinfos. This system requires Navinfo at 200Hz. If this error persists, set drone to debug mode and change publish freq in ardrone_autonomy"

But then I realized that if the waypoint file has more waypoints than ~70ish, the queue that takes in the waypoints (can be seen under the Autopilot status menu in the GUI) misses some of the waypoints when it is reading them in from the text file. When I hit clear and send, sometimes I see the right amount of waypoints stored into the queue (%30) and at other times it is always less (%70).
This is really weird. Do other people also get this issue?

EDIT 2:
Adding a print line in the RosThread::publishCommand(std::string c) function acts as a delay and all waypoints get pushed to the queue. I think a delay needs to be added or the ros publish queue needs to be extended for waypoints of size>70 to work properly.