Added debug information to USBLManager. Tweaked the USBL nodelet data sending.