cherry picking bug fix from #220 : update custom functions
cherry picking bug fix from #220 : update custom functions

Thanks @aeudes

facontidavide/PlotJuggler,facontidavide/PlotJuggler,facontidavide/PlotJuggler,facontidavide/PlotJuggler,facontidavide/PlotJuggler