AVC: Pose and Map Display

To fix the path following algorithm on Data Bus I have to know what the robot is thinking.

To know what it's thinking, the robot is now sending some new data in its telemetry stream to the GCS.

• All waypoints
• Index of the next waypoint
• Lookahead position

The GCS now opens a map window which scales and displays the data above as well as vehicle pose (position and heading).


But before I got to that point, when I first started coding up the map window, it was immediately obvious that the lookahead point was wrong. Below, the green dots are waypoints, the tiny yellow dot in the upper right is the rover, and the blue dot is the lookahead point.

Lookahead point. You're doing it wrong.

I was computing the position of the lookahead point relative to the previous waypoint, but failing to add the coordinates of that waypoint.

In my attempt to fix the problem I mistakenly added the relative distance to the robot's position. The map display made it obvious what was wrong. The lookahead point kept getting farther and farther away.

I got it fixed. Then I added vehicle heading as a line. I draw waypoints in red and the next waypoint in green. I was able to verify that the code switches to the next waypoint properly. Heading looks good. Now I can do some diagnosis on the path following.

There. That's better.

Point is, visualization is awesome. I think it will be a big time saver for troubleshooting.