Fixed several major bugs with tag processing. Also added more user interface output
Added more waypoint
Merge branch 'hw_test' of github.com:SquadCollaborativeRobotics/global_planner into hw_test
changed duration between sending goal poses