various improvements to sm servoing
updating to use older tf listener to avoid race conditions