Fixed bug in sendwaypoint and sendgoal. added debug info. changed waypoint file