modified some state transitions for waypoint navigation
modified some state transitions for waypoint navigation

SquadCollaborativeRobotics/global_planner,SquadCollaborativeRobotics/global_planner,SquadCollaborativeRobotics/global_planner