making sure robot gets to the correct pose on goal waypoint
replanning added for unexpected state transitions
handling situations where no message is published in /current_node