Added getGoal service, fixed frame_id sometimes not being set