Fix queue and add script to plot odom from topics