Modified send_goal. It has 4 modi now and can send the robot autonomous from one waypoint to the next one