Functions mostly. rviz and gazebo differ
Merge branch 'fixBugs' of github.com:SquadCollaborativeRobotics/global_planner into fixBugs