Added threshold to flyToPosition function. Added ros outputs for testing