waypoint_manager improvements and fixes

[sethmnielsen]

Changes to waypoint_manager.py. In a nutshell:

• Added heading error threshold requirement for reaching a waypoint
• Setting the rosparam cycle to false now works
  • Also skips odometryCallback entirely once reaching final waypoint, thus skipping unnecessary computations
• Improved info print statements
• Removed duplicate code
• Renamed next_waypoint back to current_waypoint - calling it next_waypoint seems confusing given that self.current_waypoint_index is used to select the waypoint, and publishing the waypoint effectively makes it the current waypoint

[sethmnielsen]

@davidcGIThub @matthewk-rydalch Have these fixes been tested yet? If so, can we merge it in?